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An  analysis  of  bias  in  the  Extended  Kalman  Filter  has  been  performed  leading  to  the 
identification  of  its  cause,  an  estimate  of  its  magnitude  and  the  synthesis  of  a filter  in  which 
this  bias  is  substantially  reduced.  These  results  are  reported  with  respect  to  a particular,  but 
very  important,  nonlinear  estimation  problem:  passive  target  tracking.  An  approach  inte- 
grating both  analytic  (mathematical)  and  empirical  (simulation)  investigations  is  used. 
Observations  made  in  simulation  studies  are  examined  mathematically  and  the  consequent 
analytic  formulations  are  validated  using  further  simulation  experiments. 

The  Extended  Kalman  Filter  is  a simple  recursive  algorithm  that  uses  a stochastic 
linearization  to  utilize  the  form  of  the  standard  linear  Kalman  Filter  equations  to  estimate 
the  state  trajectory  of  a system  when  the  dynamical  and/or  the  measurement  equations  are 
nonlinear.  The  use  of  this  linearization,  which  is  based  on  the  filter’s  a priori  state  estimate, 
leads  to  a complex  stochastic  relationship  between  the  sequence  of  gain  matrices  and  the 
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sequence  of  measurement  residuals.  The  fact  that  these  two  sequences  are  correlated  is  the 
cause  of  the  observed  bias,  and  a unique  stochastic  perturbation  analysis  yields  a prediction 
of  the  bias  introduced  into  the  filter’s  state  estimate  at  each  measurement  update. 

This  prediction  of  the  bias  and  an  estimate  of  it  based  on  quantities  available  to  the 
filter  itself  are  studied  and  modified  using  an  empirical  method.  A filter  is  synthesized  using 
these  results  and  its  performance  is  shown  to  be  a substantial  improvement  over  the  standard 
method. 

Two  other  methods  for  avoiding  the  bias  inherent  in  the  Extended  Kalman  Filter  are 
presented  and  tested. 


IX 


SECTION  I 


INTRODUCTION 

This  report  details  the  results  of  an  investigation  into  the  passive  target  tracking 
problem.  The  term  "passive"  is  used  here  to  denote  the  use  of  a passive  sensor  for  target 
detection  and  tracking.  Such  a sensor  is  one  that  intercepts  energy  radiated  by  the  target  to 
be  tracked  typically  sonic,  infrared  or  radio  frequency  emissions  (as  opposed  to  an  "active" 
sensor  that  radiates  energy  and  intercepts  the  energy  reflected  by  the  target).  This  work  was 
motivated  by  the  increasing  importance  of  passive  sensors  in  the  military  arena  due  to  the 
limitations  of  active  sensors.  The  most  important  of  these  limitations  is  the  fact  that  active 
sensors  provide  the  target  a warning  that  it  is  being  tracked  leading  to  possible  counter- 
measures that  will  degrade  the  performance  of  a weapons  system  and  may  even  lead  to  an 
attack  on  the  platform  using  the  active  sensor. 

A fundamental  shortcoming  of  passive  sensors  is  that  they  are  able  to  measure  only 
the  angle  of  arrival  of  the  incident  energy.  This  is  typically  referred  to  as  measuring  the 
"line-of-sight,"  or  LOS,  to  the  target  and  this  term  will  be  used  in  the  rest  of  this  work.  While 
the  LOS  is  the  most  important  information  required  to  track  a target,  this  single  observable 
does  not  generally  provide,  in  and  of  itself,  sufficient  information  regarding  a target  on 
which  to  base  a suitable  response.  What  is  missing  is  information  about  the  distance  between 
the  platform  and  the  target  (the  "range")  and  the  rate  at  which  this  distance  is  changing  (the 
"range-rate").  The  range  and  range-rate  provide  an  indication  of  the  target  intention,  the 
degree  of  threat  it  poses  and  whether  it  is  within  the  employment  parameters  of  a weapons 
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system.  For  example,  this  information  can  be  used  to  answer  questions  such  as:  "Is  this 
target  in  a position  to  successfully  employ  his  weapons?  Is  he  within  the  capabilities  of  my 
weapons  systems?  Is  he  in  an  aggressive  mode  (closing  on  an  intercept  course)  or  is  he 
fleeing  (attempting  to  increase  range)?"  If  these  questions  can  be  reliably  answered  using 
data  only  from  passive  sensors  then  a great  tactical  advantage  is  gained. 

The  salient  feature  of  the  passive  target  tracking  problem  is  the  nonlinear  relationship 
between  the  measurement  function,  the  LOS,  and  the  system  states,  relative  position  and 
velocity  in  cartesian  coordinates.  This  places  the  passive  target  tracking  problem  in  the 
general  class  of  nonlinear  estimation.  The  general  linear  estimation  problem  was  solved 
using  a state  space  formulation  by  Kalman  in  1960  [1]  leading  to  the  optimal  recursive 
estimator:  the  Kalman  Filter.  The  general  nonlinear  estimation  problem  has  been  intensively 
studied  but  no  such  solution  has  been  achieved  nor  does  it  seem  likely  to  be  produced  in  the 
near  future  due  to  the  richness  of  the  behavior  of  nonlinear  systems  (particularly  when  forced 
by  stochastic  processes). 

Many  approaches  to  nonlinear  estimation  have  been  investigated  including  the 
extension  of  the  standard  Kalman  Filter  algorithm  to  the  general  problem  [2,3].  A brief 
presentation  of  this  approach  is  given  in  section  3 which  introduces  the  Extended  Kalman 
Filter.  This  is  only  one  method  of  approaching  the  problem  but  it  does  have  the  advantages 
of  conceptual  and  computation  simplicity  since  it  retains  the  recursive  algorithm  of  the 
Kalman  Filter.  Other  methods  have  been  used  and  the  literature  on  their  design  and  per- 
formance is  far  too  voluminous  to  even  begin  to  cite  here. 

Because  of  the  lack  of  a general  solution,  much  effort  towards  solving  practical 
nonlinear  estimation  problems  has  been  focused  on  particular  applications.  This  tends  to 
result  in  particular  solutions  to  particular  problems  and  conclusions  reached  in  one  project 
may  have  no  bearing  on  other  applications.  The  present  work  does  have  considerable 
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potential  for  application  beyond  the  application  addressed  here  because  the  type  of  analysis 
performed  applies  to  all  forms  of  the  Extended  Kalman  Filter. 

Passive  target  tracking  is  among  the  most  studied  nonlinear  estimation  problems  in 
terms  of  time,  money  and  the  breath  of  researchers  involved  [4-32]  and  is  still  of  major 
interest.  The  work  that  has  been  accomplished  previously  on  this  problem  has  consisted  of 
various  methods  of  analysis  (all  approximate),  synthesis  (generally  heuristic)  and  simulation 
study  of  a wide  variety  of  estimators.  Much  of  this  previous  work  has  utilized  the  Extended 
Kalman  Filter  algorithm  and  its  many  possible  variants  and  others  have  attempted  to  utilize 
"full-blown"  nonlinear  estimators.  The  advantages  of  the  former— simple  recursive 
algorithms— have  made  them  the  focus  of  most  research.  Flowever,  for  all  the  work  on  this 
problem,  no  generalized  analysis  has  been  performed  on  the  properties  (convergence, 
biasedness)  of  the  estimates  of  the  proposed  filter  designs.  Even  the  ubiquitous  Extended 
Kalman  Filter  has  not  yielded  to  such  an  analysis. 

The  present  work  does  not  purport  to  perform  such  a global  analysis  but  rather  to 
achieve  an  explanation  for  a particular  failure  machinism  demonstrated  by  the  Extended 
Kalman  Filter  in  its  application  to  the  passive  target  tracking  problem  by  means  of  a specific 
and  unique  analysis.  This  failure  machanism,  the  accumulation  of  bias  in  the  state  estimate, 
will  be  demonstrated,  its  cause  identified  and  a mathematical  analysis  performed  to  deter- 
mine a formula  for  predicting  the  bias.  Given  this  formula  its  ability  to  predict  the  bias  is 
observed  and  a number  of  observations  are  made.  Finally,  having  an  estimate  of  the  bias 
an  estimator  is  synthesized  that  remove  the  bias.  This  estimator  is  demonstrated  to  exhibit 
greatly  improved  tracking  performance  compared  to  the  EKF.  Two  other  estimator  designs 
are  proposed  based  on  the  observation  of  the  cause  of  the  bias  and  their  performance  is 
likewise  evaluated. 
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The  results  of  this  investigation  are  a new  and  unique  understanding  of  the  properties 
of  the  Extended  Kalman  Filter  and  estimator  designs  that  outperform  all  previous  efforts. 
Although  these  substantial  results  are  reported  for  only  one,  albeit  crucial,  application  within 
the  general  class  of  nonlinear  estimation  the  analysis  performed  is  not  so  limited.  The 
perturbation  analysis  presented  in  section  6 can  be  applied  to  any  nonlinearity  encountered 
in  an  estimation  problem  and  as  such  may  be  applied  anywhere  the  Extended  Kalman  Filter 
yields  less  than  optimal  performance  (assuming  one  has  some  sense  of  what  is  optimal  in 
that  particular  case).  Since  the  Extended  Kalman  Filter  has  found  application  in  a wide 
range  of  estimation  and  identification  problems  the  potential  of  this  method  is  quite  broad. 
In  addition,  such  an  analysis  could  be  applied  to  the  inverse  of  the  nonlinear  estimation 
problem:  the  problem  of  controlling  a plant  when  the  dynamical  equations  are  nonlinear  in 
the  state  and/or  the  control  variables. 


SECTION  2 


PROBLEM  STATEMENT 

In  order  to  analyze  the  passive  target  tracking  problem  it  is  necessary  to  establish  the 
proper  definitions  to  be  used  in  subsequent  discussion.  A complete  listing  of  the  nomen- 
clature used  is  given  in  appendix  A.  For  simplicity  this  work  was  restricted  to  a two 
dimensional  problem.  All  observations  made  here  are  extendable  to  the  three  dimensional 
case  but  considerable  complications  are  avoided.  To  illustrate  the  situation  consider  figure 
2. 1 on  the  next  page. 

The  observation  platform,  called  here  the  "own-ship"  (to  use  the  naval  terminology), 
has  a velocity  defined  by  speed  sos  and  heading  (relative  to  the  fixed  cartesian  coordinate 
axis)  ((>,*.  The  target  moves  at  speed  ,vtg  and  heading  (J)^.  The  own-ship  measures  the  LOS 
as  defined  relative  to  the  fixed  coordinate  axis.  There  are  two  possible  approaches  to  defining 
the  state  vector.  The  first  method,  which  is  used  throughout  this  research  project,  is  to  use 
the  cartesian  coordinates  x,  y and  their  derivatives  x,y  as  the  four  state  variables  (extension 
to  six  state  variables  using  the  accelerations  is  possible  but  not  used  here).  A second  method 
is  to  use  "modified  polar  coordinates"  [18,23,27,30,33]  which  produces  a much  more 
complicated  set  of  states  equations  and  is  not  used  (investigations  revealed  no  advantage  to 
this  method). 

Using  the  cartesian  definition  makes  the  state  dynamics  linear  and  places  the  non- 
linearity in  the  measurement  equation.  More  exactly  the  state  vector  is  defined  as 

x = [x  y x y]r  (2.1) 
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Figure  2.1  Geometric  Definitions 
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so  the  state  dynamical  equation  is 


£*  = <&*-i£*-i  + w* 

where  <3>A:_1  is  the  state  transition  matrix, 

'1  0 A tk  O' 

^ 0 1 0 At 

0 0 1 0 

0 0 0 1 

and 

At* 


(2.2) 


(2.3) 


The  system  is  shown  to  be  forced  by  a discrete  random  process  wk  which  is  assumed  to  have 
statistics 

£[h>*]  = 0 (2.4) 

E[wkwJ\  = Qkhu  (2.5) 

The  matrix  Qk  is  referred  to  as  the  process  noise  covariance  matrix.  This  random  process 
is  used  to  "model"  any  unmodeled  dynamics  (primarily  target  accelerations)  and  the  effect 
of  system  nonlinearities  (as  in  the  measurement  equation).  This  is  the  most  rudimentary 
method  of  allowing  for  target  accelerations  and  many  other  more  sophisticated  methods  are 
available.  However,  the  following  work  is  not  dependent  the  target  modeling  scheme.  Using 
this  scheme  is  essentially  a worst  case  scenario  and  further  improvements  may  be  made 
using  other  modeling  methods.  It  should  be  stressed  though  that  the  source  of  the  problems 
addressed  in  this  work  are  not  the  result  of  improper  target  modeling  but  with  the  nature  of 
the  filter  itself. 

Finally,  the  measurement  equation  is 
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(2.6) 


where  vk  is  the  measurement  noise  sequence  with  statistics 


£[vj  = 0 


(2.7) 


E[vkvf]=Rk8u 


(2.8) 


The  matrix  Rk  is  called  the  measurement  noise  covariance  matrix  (though  in  this  case  it  is 
a scalar).  The  measurement  noise  v*  is  in  general  assumed  to  be  gaussian  and  that  assumption 
will  be  used  in  the  simulation.  Given  the  physical  nature  of  the  measurement  noise  in  a real 
sensor  system  this  is  a good  assumption.  This  simple  problem  definition  encompasses  all 
the  relevant  passive  tracking  problems  addressed  in  the  literature. 

The  subject  of  the  "observability"  in  the  passive  target  tracking  problem  is  of 
importance.  In  a linear  estimation  problem  the  observability  of  the  system  states  is  com- 
pletely determined  by  the  system  and  measurement  matrices  and  is  not  a function  of  the 
actual  state  trajectory.  In  this  particular  nonlinear  problem  this  is  not  the  case.  Observability 
is  now  dependent  on  the  state  trajectory  itself  and  not  just  the  system  equations.  A criteria 
for  the  observability  of  a particular  trajectory  has  been  derived  [15]  and  can  be  summarized 
as  follows. 

Assume  that  the  time  derivatives  of  the  target  position  of  all  orders  greater  than  n are 
zero.  Then  the  range  will  be  observable  if  and  only  if  the  relative  motion  between  the  target 
and  own-ship  is  such  that  the  n+1  derivative  of  the  LOS  is  nonzero.  For  example,  if  the 
target  is  stationary  so  that  the  first  and  all  higher  derivatives  of  its  position  (velocity, 
acceleration  etc.)  are  zero  then  the  range  is  observable  if  and  only  if  the  LOS  rate  (first 
derivative)  is  nonzero.  The  second  and  higher  derivatives  of  the  LOS  can  be  zero.  If  the 
target  is  moving  at  a constant  velocity  (second  derivative  and  higher  are  zero)  then  the 
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second  derivative  of  the  LOS  must  be  nonzero.  This  typically  requires  an  own-ship 
acceleration. 

This  definition  of  observability  is  qualitative  rather  than  quantitative  and  thus  certain 
trajectories  are  said  to  be  more  or  less  observable  without  any  magnitude  being  stated.  In 
general  a trajectory  is  termed  more  observable  if  the  LOS  rate  is  high  and  changing  and  to 
have  lower  observability  otherwise.  The  most  observable  trajectories  are  in  general  those 
in  which  the  own-ship  makes  frequent  course  changes  (typically  employing  a zigzag 
maneuver  pattern).  As  would  be  expected,  when  the  trajectory  is  highly  observable  the 
estimation  covariance  of  an  estimator  will  decrease  rapidly  as  more  measurements  are 
processed  and  this  can  be  considered  to  be  the  actual,  practical  definition  of  observability 
in  this  context.  Thus  it  can  be  stated  that  observability  in  this  context  is  essentially  a phe- 
nomenological quality  as  opposed  to  an  analytically  definable  quantity. 


SECTION  3 


THE  EXTENDED  KALMAN  FILTER 


The  pioneering  work  of  Kalman  [1]  in  the  area  of  optimal  estimation  of  the  states  of 
a linear  system  given  noisy  measurements  of  a linear  combination  of  the  states  is  without 
doubt  the  most  important  single  achievement  in  control  and  estimation  theory.  It  was  perhaps 
inevitable  that  this  success  would  lead  to  attempts  to  apply  the  fundamental  results  of  Kalman 
Filtering  to  the  case  of  nonlinear  systems  or  nonlinear  measurement  functions  (or  both). 
This  "extension"  of  the  linear  theory  has  lead  to  the  formulation  of  the  Extended  Kalman 
Filter  (EKF)  which  is  at  the  base  of  most  of  the  work  done  on  passive  target  tracking.  First 
a synopsis  of  the  results  of  the  linear  filter  theory  will  be  presented  and  then  the  EKF 
introduced. 

Consider  an  n^-order  linear,  continuous  dynamical  system  with  n x 1 state  vector  x(t) 
satisfying  the  equation 


^p-  = F(t)x(t)  + w(t) 


(3.1) 


where  w(t)  is  a stochastic  process  with 


£[w(»]=0 


(3.2) 


£[w(Ov/(x)]  = {2(05(f-i) 


(3.3) 
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No  deterministic  control  input  is  shown  though  it  is  implied  that  it  exists  and  is  perfectly 
known  and  thus  can  be  dropped  from  the  derivation.  The  initial  conditions  on  the  state 
estimate  are 

£[*o]  =io  (3.4) 

£I*o*o]=-po.o  (3-5) 

Discrete  linear  measurements  are  taken  at  times  tk=kT  = 0 ,T  ,2T .... 

zk=H(tk)x(tk)  + vk  (3.6) 

where 

E[vk]=0  (3.7) 

E[vkvf]=Rk  8W  (3.8) 

The  Kalman  Filter  provides  a minimum  variance  estimate  of  the  state  vector  given  the  initial 
conditions  and  measurement  sequence.  It  does  this  recursively  by  predicting  an  a priori 
state  estimate  x k k_k  at  time  k and  its  own  estimate  of  its  estimation  covariance 

= ^it  - i,jt — (3.9) 

^k.k- 1 = ^k.k-l^k-l.k-l^k.k-l  + Qk  (3.10) 

Then  the  estimates  are  updating  using  the  following  equations 

Kk  = Pk.k-tf  [HkPkk_X +RkTl  (3.11) 

*_k,k  =*_k,k- 1 +Kk(zk  ~ Hk*ik,k-l)  (3.12) 

Pk_k  = (I  - Kk  Hk)Pk  k _j(7  - Kk  Hk)T  + Kk  Rk  k£  (3.13) 

where  the  State  Transition  Matrix  O*  k_l  is  defined  by  the  matrix  differential  equation 
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dt 


=FmkJ 


(3.14) 


with  initial  condition 


and  the  property  that 


<|>.  .<!>  . = <J>,  . 

k,j  j,i  k,i 


(3.15) 


(3.16) 


The  sequence  wk  is  a zero  mean  white  gaussian  with 

E[wkwTk]  = Qk  = j' 


(3.17) 


This,  in  summary,  is  the  Kalman  Filter. 

Now  consider  the  nonlinear  nrt-order  system  specified  by  the  continuous  nxl  state 
vector  x(t ) satisfying  the  dynamical  equation 

dx(t) 

=fix(t),t)  + w(jt ) (3.18) 

where  w(t)  is  a gaussian  stochastic  process  as  defined  in  (3.2)  and  (3.3)  and  the  initial 
conditions  on  the  state  estimate  are  also  as  shown  in  (3.4)  and  (3.5).  However,  the  discrete 
measurements  are  also  nonlinear. 


zk=h(x(tk))  + vk 

with  vk  as  defined  in  (3.7)  and  (3.8). 


(3.19) 


In  order  to  apply  linear  Kalman  filter  theory  the  above  system  can  be  linearized  along 
some  "nominal"  or  "reference"  trajectory  (call  itx(t)).  One  option  is  to  choose  ax(t)which 
satisfies  the  dynamical  equation 

r=f(x(t),t ) (3.20) 
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with  initial  condition  x0  = x0.  This  makes  x(t)  deterministic  so  that  anything  that  is  a function 


of  x(t ) can  be  written  as  a function  of  x(t0). 

Linearizing  about  x(t ) yields  the  continuous  linear  system 


dx(t ) 
dt 


= F(x(t0),t)x(t)  + w(t) 


(3.21) 


where 


F(x(t0),t)  = 


df(x,t ) 
dx 


(3.22) 


£=i(0 


Consider  the  deviation  of  the  nominal  trajectory  from  the  actual  defined  as 

x(t)=x(t)~x(t)  (3.23) 

Substituting  (3.18)  and  (3.20)  into  (3.23)  it  is  obvious  thatx(t)  is  a stochastic  process  sat- 
isfying the  differential  equation 

dx(t) 


dt 


■ t)  ~f(x(t0),  t ) + w(t) 


(3.24) 


with  initial  conditions 


£[£('o)]  =*<)-*«  = 0 

E[x(t0)xT(t0)]  = P00 

Using  the  partial  derivative  defined  in  (3.22)  yields  the  standard  approximation 

f(x(t),  t ) ~f(x(t0),  t)  = F(x(t0),  t)x(t) 

Substituting  (3.27)  into  (3.24)  gives  the  perturbation  equation  for  x(t) 

dx(t) 


(3.25) 


(3.26) 


dt 


= F(x(t0),t)x(t)  + w(t ) 


(3.28) 


14 


The  x(t0 ) argument  in  F will  now  be  dropped  since  it  is  a function  of  t only.  This  equation 
can  be  discretized  to  obtain 


and  where  the  State  Transition  Matrix  <J>*  and  wk  are  in  (3.14),  (3.15),  (316)  and  (3.17) 

though  they  are  now  functions  of  the  linearizing  trajectory  x(t).  Calculating  the  state 
transition  matrix  is  in  general  very  difficult  and  various  approximate  methods  can  be  used. 
That  problem  will  not  be  dealt  with  here  since  in  the  present  problem  the  state  dynamics 
are  linear. 

The  measurement  equation  can  likewise  be  linearized  by  defining  a nominal  mea- 
surement sequence 


**,*-i— *-i + wk 


(3.29) 


with  the  initial  condition 


x0=x(t0) 


(3.30) 


zk=h(xk,tk) 


(3.31) 


and 


Z-k  zk  zk 


(3.32) 


Then 


zk~H(x(tk),tk)xk  + vk 


(3.33) 


where 


£ = £('*) 


A 


(3.34) 


Thus  we  have  a discrete  linear  system 


(3.35) 
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zk=Hkxk  + vk  (3.36) 

to  which  the  linear  Kalman  filter  equations  can  be  applied  to  obtain  (approximately)  a 
minimum  variance  estimate  of  x(t)  which  since  x(t)  is  known  yields  an  estimate  of  x(t). 


Equivalently  the  equations, 

?Lk,k-\ ~ k)?Lk-i,k-i  (337a) 

Pk.k- 1 = — — *)  Qk  (3.31b) 

+ 031c) 

ik.k=kk.k-i  + Kk(zk-h(x,3-Hk(xk)(xktk_l-2k))  (3.31d) 

P^  = (/-K,//t(it))PM_1(/-Ki//i(xi))r  + K,/?,<  (3.37e) 


yield  this  estimate.  The  dependence  of  all  the  linearized  terms  onrk  has  been  explicitly 

shown.  These  equations  are  called  the  linearized  Kalman  Filter  equations  with  nominal 
trajectory  x(t).  Note  that  there  are  three  terms  in  the  innovations  sequence 
zk-h(xk)-H(xk)(xkk_l-xk).  The  first  two  are  the  same  as  in  the  standard  Kalman  Filter 
(though  they  are  now  nonlinear  functions),  zk  is  the  measurement  and  -h(xk)  is  the  negative 
of  the  predicted  measurement  based  on  the  nominal  state  trajectory.  The  third  term, 
~Hk(xk)(xkk_l -xk)),  is  the  negative  of  the  difference  between  the  a priori  estimate  and 
the  linearizing  state  trajectory  projected  into  the  measurement  coordinates  by  the  gradient 
of  the  measurement  function  evaluated  on  the  linearizing  trajectory.  This  term  does  not 
appear  in  the  Extended  Kalman  Filter  because  the  a priori  state  estimate  and  the  linearizing 
state  trajectory  are  identical.  In  two  of  the  later  filter  designs  (sections  10  and  12)  this  "third 
term"  will  be  reconsidered  because  different  linearizing  trajectories  are  used. 
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The  Extended  Kalman  Filter  (EKF)  uses  its  own  a priori  estimate  of  the  state  in  the 
linearized  Kalman  Filter  equations,  that  is  x k is  replaced  by  x k k_k.  The  EKF  equations  are 
thus  summarized  as 

X.k,k-l=<^>k,k-lX.k-l,k-l  (3.38  a) 

^k.k- 1 =^k,k-lPk-l,k-l*bk,k-l  + Qk  (3.38  b) 

^k  = ^k.k  - 1 Hk  (Hk  ^k,k-l^k  + ^<fc)  (3.38c  ) 

ILk.k  =*Lk,k- 1 Kk{zk  ~ h(x^k  k_1))  (3.38J) 

Pk,k  = (/ -KkHk)Pkk_x  (1  -KkHf  + KkRkKk  (3.38*) 

where 


3/i(x) 


Hk  dx 


(3.39) 


±-±k,k- 1 


It  is  obvious  that  the  linearization  of  a nonlinear  measurement  function  is  exact  only  at  the 
point  of  linearization. 

It  has  been  proposed  [22,31]  that  rather  than  approximating  the  total  variation  of  a 
nonlinear  measurement  function  at  time  tk  by  its  first  variation  it  be  replaced  exactly  by  a 
linear  structure  that  is  a function  of  the  state  estimate,  xktk_1,  and  the  actual  measurement 
function,  h(x).  A function  for  which  such  a linear  structure  can  be  found  is  called  a mod- 
ifiable function.  To  illustrate  consider  approximating  the  noise  free  measurement,  z,  by  its 
first  order  expansion, 

dh  (x) 


z = h (x)  ~ h (x ) + 


dx 


(x-x) 


(3.40) 


so  that 
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h(x)-h(x)~^^-  (x-x)  (3.41) 

£ = £ 

If  h (x)  is  a modifiable  function  then  an  exact  relationship  can  be  can  be  written  as 

h(x)-h(x)  = g(z\x)(x-x)  (3.42) 

The  Modified  Gain  Extended  Kalman  Filter  (MGEKF)  uses  the  modified  form  of  the 
measurement  function  evaluated  at  the  current  measurement  and  the  a priori  state  estimate, 
8 fa’*  k,k  - 1)>  in  the  covariance  update  equation  while  retaining  all  the  other  equations  of  the 
EKF.  That  is,  the  standard  update  equation, 

Pk.k  = ~^kHk(xkk_l))P -KkHk(^kk_^))T +HkRkHl  (3.43) 

is  replaced  by 

= + (3-44) 

It  should  be  noted  that  the  derivation  and  analysis  of  the  MGEKF  [22,3 1]  involves  the  noise 
free  measurement  z but  in  actual  implementation  the  measurement  zk  is  used.  This  analysis 
resulted  in  the  claim  that  the  MGEKF  is  unbiased  but  using  the  actual  measurement 
invalidates  this  conclusion. 

It  is  also  important  to  note  that  the  modified  form  is  not  used  in  calculating  the  gain 
Kk.  This  would  cause  the  gain  to  be  a function  of  the  current  measurement  and  thus  have  a 
strong  correlation  with  the  measurement  residue  sequence  resulting  in  poor  performance. 
The  MGEKF  has  been  shown  to  yield  better  performance  than  the  EKF  for  some  applications 
and  conditions  [22,31,33]  and  has  been  claimed  to  be  unbiased.  The  MGEKF,  however, 
suffers  from  the  same  bias  as  the  EKF  (for  the  same  reason).  The  mathematical  analysis  in 
the  next  section  for  the  EKF  applies  to  the  MGEKF  as  well  under  the  assumptions  made. 


SECTION  4 


PREVIOUS  WORK 

A vast  body  of  reported  research  has  been  accomplished  in  the  area  of  passive  target 
tracking.  As  stated  earlier  the  focus  of  the  research  has  been  based  on  the  EKF  and  its 
diverse  modifications  and  has  consisted  of  simulation  studies  to  determine  estimator  per- 
formance under  various  conditions  of  observability,  measurement  noise,  target  acceleration 
modeling  and  actual  target  accelerations.  In  addition,  numerous  variations  of  the  EKF  have 
been  proposed  involving  different  choices  for  the  state  variables  [18,27,30,32],  attempts  to 
improve  the  linearization  method  [22,28,31,33]  and  a wide  array  of  ad  hoc  "fixes" 
[14,24,25,26].  These  attempts  have  been  made  because  the  standard  cartesian  EKF  for- 
mulated for  this  problem  at  the  end  of  the  last  previous  section  is  widely  understood  to  yield 
poor  performance  when  applied  under  adverse  conditions  - large  initial  estimation  error 
covariance,  excessive  measurement  noise  and  target  accelerations.  Underlying  these  efforts 
have  been  the  unasked  (and  certainly  unanswered)  questions:  (/)  what  is  the  failure  mode 
of  the  EKF  in  these  situations,  (ii)  given  the  answer,  what  is  the  cause?  And  possibly  the 
most  important  question  is,  can  anything  be  done  to  rectify  it?  Despite  the  major  activity 
in  the  area,  there  have  been  no  conclusive  answers  to  these  basic  questions. 

The  accomplishment  of  the  work  documented  here  is  to  answer  these  questions  and 
the  most  important  result  has  been  the  formulation  of  three  conceptually  simple  estimators 
based  on  the  cartesian  EKF  that  provide  effectively  unbiased  and  near  minimum  variance 
estimates  of  the  range  and  range-rate  in  cases  where  the  standard  filter  fails.  These  filters. 


18 


19 


rather  than  being  ad  hoc  modifications  of  the  cartesian  EKF,  are  based  on  a mathematical 
analysis  of  the  interactions  inherent  in  the  EKF  and  as  such  represent  considerable  progress 
towards  the  goal  of  optimal  passive  target  tracking.  Because  the  modifications  are  based 
on  a mathematical  justification,  they  can  be  considered  to  be  more  globally  valid  than 
previous  ad  hoc  approaches. 

The  remainder  of  this  dissertation  will  present  the  major  mathematical  and  simulation 
results  in  an  order  designed  to  illustrate  the  logical  progression  of  empirical  and  analytical 
discoveries.  The  initial  empirical  observations  motivate  a mathematical  investigation  which 
in  turn  is  validated  and  modified  using  specific  empirical  experiments.  This  inquiry  cul- 
minates in  the  synthesis  of  three  filter  designs  and  performance  analyses  that  demonstrate 
their  superior  performance.  The  results  are  not  necessarily  given  in  the  order  in  which  they 
were  discovered  but  rather  in  an  order  that  provides  greatest  clarity.  As  in  any  basic  research 
effort  many  results  were  obtained  before  the  proper  context  was  realized  to  make  them 
coherent. 

To  make  a proper  analysis  of  the  usefulness  of  the  filter  designs  presented  here,  a 
criterion  for  determining  whether  the  designs  are  advantageous  enough  to  be  worth  the  effort 
is  required.  First  it  should  be  reiterated  that  the  standard  cartesian  EKF  does  perform  quite 
well  in  cases  where  the  initial  estimate  is  good,  the  measurements  are  good  (low  measurement 
noise  covariance),  a highly  observable  trajectory  and  little  or  no  process  noise.  In  these 
cases  the  EKF  can  be  used  without  modification.  However,  in  many  cases  these  ideal 
conditions  are  not  obtainable  and  it  is  desired  to  maximize  estimator  performance.  In  such 
non-optimal  conditions  the  standard  EKF  is  known  to  fail  and  in  the  next  section  it  will  be 
made  clear  why  this  is  so. 

A criteria  is  required  judging  the  proposed  estimator  designs  since  it  may  be  that  no 
realizable  filter  can  do  any  better  and  thus  there  is  no  point  in  any  further  effort.  The  answer 
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to  this  problem  is  to  determine  the  Cramer-Rao  lower  bound  (CRLB)  for  the  particular 
estimation  problem  at  hand  and  this  is  provided  by  what  will  referred  to  as  "Ideal  Extended 
Kalman  Filter  (IEKF)." 

The  IEKF  can  best  be  summarized  as  the  EKF  linearized  about  the  actual  state  tra- 
jectory. That  is,  where  the  EKF  uses  the  gradient  of  the  nonlinear  measurement  function 
h(x)  evaluated  at  the  a priori  state  estimate  xM_,  the  IEKF  evaluates  the  gradient  at  the 
actual  state*  t.  Using  this  linearization  in  (3.37c)  and  (3.37e)  yields  deterministic  gain  and 
covariance  matrix  sequences  and  it  has  been  shown  [8]  that  in  the  absence  of  nondeterministic 
inputs  the  resulting  covariance  matrix  (call  it  Pk ) is  the  CRLB  for  the  estimation  problem. 
That  is,  for  any  realizable  estimator  with  (actual)  covariance  matrix  Pk, 

Pk  - Pk  is  positive  definite  V k 

This  property  will  be  used  to  assess  the  performance  of  all  the  filters  considered  here. 

While  it  is  important  to  know  a filter’s  estimation  performance  relative  to  the  CRLB, 
it  is  also  imperative  to  know  how  well  the  mean  of  the  estimate  tracks  the  actual  value  of 
the  state.  This  is  not  difficult  in  the  case  of  linear  estimation  because  in  general  the  best 
linear  unbiased  estimator  (BLUE)  is  also  a minimum  variance  estimator.  This  is  exactly 
the  breakthrough  of  the  Kalman  Filter  because  it  provides  an  unbiased,  minimum  variance 
estimate  and  does  so  with  an  efficient  recursive  algorithm.  However,  in  the  case  of  nonlinear 
problems  no  such  general  solution  has  been  achieved  and  it  is  well  known  that  a minimum 
variance  estimate  is  not  necessarily  unbiased  and  an  unbiased  estimator  is  not  necessarily 
minimum  variance.  It  can  not  be  stated  categorically  that  an  estimator  is  unbiased  or 
minimum  variance  but  rather  the  IEKF  will  be  used  to  determine  the  lower  bound  for  the 
situations  simulated  and  the  mean  and  standard  deviation  of  the  filter  estimates  will  be 
compared  with  this  bound.  As  a rule  of  thumb,  an  estimator  will  be  considered  to  be  biased 
if  the  mean  of  its  estimation  error  is  a significant  fraction  of  its  la  error.  Like  the  subject 
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of  observability,  the  evaluation  of  biasness  will  be  qualitative  rather  than  quantitative. 

The  positive  definiteness  of  the  difference  P - P’will  not  be  examined  but  rather  the 

position  and  velocity  covariances  projected  into  cartesian  coordinates  will  be  considered. 
That  is,  consider  the  row  vector  H(x)  which  is  the  gradient  of  the  measurement  function 
h(x).  Considering  the  quadratic  form  HPHT  (arguments  and  time  sample  subscripts  will 
now  be  dropped)  it  is  evident  that  this  is  the  estimation  covariance  of  the  angular  estimation 
error  derived  from  the  cartesian  covariance  matrix  P.  It  follows  that  if  P -P*  is  positive 
definite  then, 

hpht>hp'ht 


and  this  will  be  the  criteria  used  in  subsequent  evaluations.  This  approach  is  also  applied 
to  the  range  estimation  error  covariance  by  defining  a vector  that  is  the  gradient  of  the  range 
function  g (x)  = (x2  + y 2)1/2, 


G(x)  = 


dx 


SECTION  5 


INITIAL  SIMULATION  RESULTS 

This  section  will  introduce  in  detail  the  simulation  scenario  used  for  filter  comparisons. 
The  simulation  uses  the  geometric  definitions  shown  in  figure  2. 1 and  in  keeping  with  the 
naval  (ship-to-ship)  example  the  specific  setup  conditions  are  as  follows: 

Initial  ownship  position:  (0,0)  in  inertial  coordinates  (nautical  miles) 


Initial  ownship  velocity: 

15  knots  (nautical  miles  per  hour) 

Initial  ownship  heading 

0°  (right  along  the  jc-axis) 

Ownship  maneuvers: 

command  to  heading  + 45°at  30  seconds 

command  to  heading  - 45°at  6 minutes 
command  to  heading  + 45°at  12  minutes 
command  to  heading  - 45 °at  18  minutes 

Ownship  turn  rate  limit: 

3°  per  second 

Initial  Target  Position: 

(0,3)  in  inertial  coordinates  (nautical  miles) 

Initial  Target  Velocity: 

10  knots 

Initial  Target  Heading: 

+ 45° 

Target  maneuvers: 

Target  does  not  maneuver 

Measurement  (update)  period: 

At  = 1 second 

Measurement  Noise  Covariance: 

Rk  = .0049  ( radians  )2  ( 1 o = 4°,  use  in  the  filter 
and  the  simulation) 
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Process  Noise  Covariance  Matrix: 


Initial  Filter  Covariance: 


Initial  Filter  Estimates: 


'0 

0 

0 

O' 

Qk  = 

0 

0 

0 

0 

0 

0 

0 

0 

_0 

0 

0 

0 

(first  set) 

'0 

0 

0 

0 

Qk  = 

0 

0 

0 

0 

0 

0 

1/9 

0 

_0 

0 

0 

1/9 

(second  set) 


'.25  0 0 0 ' 

= 0 .0441  0 0 

°“  0 0 100  0 

. 0 0 0 100_ 

xo=xk+N(0,pn) 


yo=yk+H(0,p22) 

*o  = *k+N(0,Pn) 

$o=yk+N(Q’Pu) 

The  actual  initial  conditions  and  the  filter’s  initial  covariance  have  been  made 
commensurate  as  well  as  the  measurement  covariance.  The  own-ship  motion  is  deterministic 
and  the  simulation  lasts  for  21  minutes.  The  initial  seed  for  and  the  number  of  calls  to  the 
random  number  generator  have  been  equalized  for  all  the  filters  so  that  each  filter  is 
identically  initialized  and  receives  the  same  measurement  sequences.  The  filters  are  run  in 
Monte  Carlo  sets  of  100  runs  and  their  estimates  are  averaged  over  this  ensemble  to 
approximate  the  statistical  mean. 

The  EKF,  MGEKF  and  the  IEKF  were  all  run  in  Monte  Carlo  sets  as  described.  To 
compare  performance  two  standard  plots  will  be  shown  for  each  filter  considered  (including 
the  three  filters  proposed  in  subsequent  sections).  The  first  is  the  ’Range  Tracking  Per- 
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formance  Plot.’  This  consists  of  the  averaged  range  estimation  error  (actual  range  minus 
estimated  range,  solid  line),  the  filter’s  estimate  of  its  la  range  error  (averaged  over  the  set, 
dotted  line)  and  the  actual  la  of  the  range  error  (dashed  line).  The  second  standard  plot 
will  be  the  actual  (solid)  and  estimated  closing  velocities  (dotted).  The  closing  velocity  is 
simply  the  negative  of  the  range-rate. 

The  range  tracking  performance  of  the  EKF  with  zero  process  noise  covariance  matrix 
is  shown  in  figure  5. 1 (next  page).  From  this  plot  it  is  seen  that  the  average  range  error 
exhibits  a definite  bias  as  the  run  proceeds  peaking  at  .5  nmi.  at  6 minutes,  42  seconds  (about 
halfway  through  the  second  turn).  At  this  point  the  actual  la  of  the  error  is  approximately 
.7  nmi  so  the  average  error  is  about  70%  of  the  standard  error  which  should  be  considered 
to  be  at  least  moderately  significant.  The  second  turn  provides  sufficient  observability  to 
remove  the  accumulated  bias  and  greatly  decrease  the  error  standard  deviation  (to  about  .07 
nmi  at  the  end).  Note  that  the  actual  and  (averaged)  estimator  la  bounds  coincide  quite 
well  throughout  the  run  (though  individual  realizations  show  considerable  variations  about 
the  mean).  It  is  this  initial  observation  of  the  bias  that  motivated  the  present  research  project. 

A look  at  figure  5.2  (page  26)  shows  that  the  range-rate  is  also  biased  though  not  to  a 
great  extent  (about  3 knots  at  5 minutes).  This  is  an  important  observation  since  an  error 
in  range-rate  is  integrated  in  the  dynamical  equation  to  produce  an  error  in  the  range  estimate. 

Running  the  same  simulation  set  for  the  MGEKF,  the  results  show  that  under  these 
conditions  its  performance  is  similar  to  that  of  the  EKF  but  that  it  has  both  greater  bias  and 
greater  la  error  (figure  5.3,  page  27).  The  velocity  performance  (figure  5.4,  page  28)  also 
indicates  a greater  bias  than  the  EKF. 

The  results  when  repeating  the  same  set  using  the  IEKF  are  shown  in  figures  5.5  and 
5.6  (pages  29  and  30).  The  IEKF  yields  essentially  unbiased  estimates  (the  maximum  error 
of  less  than  .2  nmi  is  less  than  40%  of  the  standard  error)  and  figure  5.5  shows  that  its 
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Range  Tracking  Performance  (Error  in  nautical  miles  with  1 sigma  bounds) 


Figure  5. 1 EKF  Range  Tracking,  q = 0 
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Actual  and  Estimated  Closing  Velocities 


Figure  5.2  EKF  Velocity  Tracking,  q = 0 
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Time  (Minutes) 


Figure  5.3  MGEKF  Range  Tracking,  q = 0 
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Time  (minutes) 


Figure  5.4  MGEKF  Velocity  Tracking,  q = 0 
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Figure  5.5  IEKF  Range  Tracking,  q = 0 
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Actual  and  Estimated  Closing  Velocities 


Figure  5.6  IEKF  Velocity  Tracking,  q = 0 

own  error  covariance  satisfies  the  Cramer-Rao  lower  bound  as  has  been  reported  previously 
[34],  Comparing  figures  5.1  and  5.3  with  this  CRLB  it  is  seen  that  the  covariance  of  the 
EKF’s  range  estimation  error  is  approximately  equal  to  the  comparable  lower  bound.  This 
suggests  that  the  EKF  is  performing  quite  well  save  for  the  obvious  bias.  The  MGEKF  does 
not  perform  as  well  as  the  EKF  in  this  case  though  as  will  be  seen  next  it  does  somewhat 
better  when  the  process  noise  covariance  matrix  is  made  nonzero  so  that  maneuvering  targets 


can  be  tracked. 
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The  results  so  far  show  that  for  a target  that  is  modeled  as  unmaneuvering  (and  that 
does  not  maneuver)  the  EKF  and  MGEKF  suffer  from  a bias  that  makes  their  estimates  less 
than  reliable  for  determining  the  target  position  though  the  particular  trajectory  provides 
adequate  information  to  eventually  identify  the  target  state.  However  we  can  not  be  assured 
that  the  target  will  not  change  speed  or  course  nor  can  we  accurately  model  such  maneuvers. 
A considerable  amount  of  work  has  been  done  attempting  to  model  the  unknown  dynamics 
of  target  maneuver  [13,29,35-41]  but  all  suffer  from  the  same  serious  flaw:  the  target  may 
not,  or  actually  it  probably  will  not,  behave  consistently  with  the  model.  Thus  any  modeling 
method  can  be  criticized  in  some  manner  as  not  corresponding  to  the  actual  dynamical 
situation. 

The  subject  of  the  present  research  is  to  determine  the  failure  machanism  in  the  standard 
approaches  to  the  Passive  Target  Tracking  problem  (the  EKF  et.  al.)  so  the  target  modeling 
issues  are  not  primary  to  this  work.  Nevertheless,  it  is  required  that  some  method  for  allowing 
for  target  maneuvers  be  implemented  to  bring  the  work  closer  to  real  application  and  to  fully 
demonstrate  the  significance  of  the  discovery  of  the  bias.  All  target  modelling  methods  that 
have  been  investigated  including  adaptive  filters,  event  driven  filters,  input  estimation, 
multiple  model  filters  and  interacting  multiple  model  filters  (among  others)  can  be  applied 
in  addition  to  the  present  work  in  order  to  improve  overall  estimator  performance.  It  must 
be  emphasized  though  that  the  bias  is  not  caused  by  erroneous  modelling  methods,  the  above 
results  were  obtained  with  a filter  that  exactly  modeled  the  target  acceleration  in  addition 
to  using  the  correct  initial  covariances  on  the  state  estimate  and  the  actual  measurement 
noise  covariance  model.  The  bias  is  endemic  to  the  EKF  and  any  other  filter  that  utilizes  a 
similar  recursive  stochastic  linearization  method. 

To  bring  this  work  closer  to  real  application  and  to  demonstrate  the  relevance  of  the 
discovery  of  the  bias  we  use  the  simplest  acceleration  model  possible,  the  one  introduced 
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in  section  3.  We  model  the  target  velocity  in  cartesian  coordinates  as  a random  walk  by 
adding  a fixed  magnitude  to  the  filters’  velocity  error  covariance  estimate  via  the  process 
noise  covariance  matrix 


Qk  = 


'0 

0 

0 

0 


0 

0 

0 

0 


0 

0 

#33 

0 


O' 

0 

0 

#44 


where  #»  = #44  = # • This  q essentially  adds  a la  uncertainty  of  q112  into  the  velocity 


covariances  between  each  measurement  update.  In  the  above  simulations  q had  been  set  to 
0 but  for  the  following  q is  set  so  as  to  introduce  a 1 a uncertainty  of  1/3  knots  each  second. 
To  demonstrate  the  reasonableness  of  this  value  consider  that  if  the  initial  error  covariance 
were  zero  and  this  value  of  q was  used  in  the  covariance  extrapolation  equation  (with  no 
measurement  updates)  that  after  1 minute  the  velocity  covariances  would  be  (2.58)2  knots2. 
It  is  not  unreasonable  to  consider  that  a typical  target  could  change  its  velocity  by  this  much 
over  the  course  of  1 minute  and  this  is  in  fact  a somewhat  conservative  value. 

Implementing  this  target  model  in  the  EKF  and  rerunning  the  simulation  set  the  range 
tracking  performance  shown  in  figure  5.7  (next  page)  is  obtained.  The  bias  is  much  worse 
and  though  it  (and  the  error  covariance)  is  reduced  by  the  frequent  own-ship  maneuvers  the 
filter’s  estimates  suffer  a recurring  error  the  makes  the  range  estimate  very  unreliable.  For 
example,  at  approximately  18  minutes  the  average  range  error  is  1 nmi.  while  the  actual  la 
of  the  error  is  .5  nmi.  This  certainly  satisfies  our  definition  of  "significandy  biased"  and  it 
is  in  fact  certain  that  such  estimates  would  be  of  no  value  in  the  type  of  decision  making  for 
which  they  are  required.  It  is  very  important  to  note  that  the  bias  reappears  after  each  of 
the  ownship  turns.  This  is  the  unavoidable  consequence  of  modelling  the  target  as 
maneuvering  since  the  filter’s  covariance  matrix  does  not  become  small  as  before.  The 
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Range  Tracking  Performance  (Error  in  nautical  miles  with  1 sigma  bounds) 


Figure  5.7  EKF  Range  Tracking  q * 0 

range-rate  also  exhibits  evidence  of  bias  as  shown  in  figure  5.8  (next  page). 

For  completeness  the  tracking  plots  for  the  MGEKF  are  included  in  figures  5.9  and 
5.10  (pages  35  and  36)  and  it  is  seen  that  the  MGEKF  now  does  somewhat  better  than  the 
EKF  but  the  bias  is  still  quite  obvious. 

For  a final  comparison  that  will  be  used  later  the  same  filters  were  rerun  in  the  same 
scenario,  but  with  a change  in  the  initialization.  In  this  case  the  initialization  is  the  same 
except  that  the  initial  range  is  set  to  the  actual  range  plus  and  minus  2 a (a  = .5  nmi)  still 
using  the  nonzero  q value.  These  sets  show  the  ability  of  these  filters  (and  the  ones  to  be 
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proposed  later)  under  very  stressing  conditions  and  how  well  they  perform  in  their  primary 
function:  to  remove  a large  initial  range  error.  Only  the  range  tracking  plots  are  shown  for 
brevity  and  the  actual  covariance  is  not  shown  (it  is  meaningless  since  the  initialization  is 
deterministic).  The  results  using  the  EKF  are  given  in  figures  5.1 1 and  5.12  (pages  37  and 
38)  and  for  the  MGEKF  in  figures  5.13  and  5.14  (pages  39  and  40). 


Actual  and  Estimated  Closing  Velocities 


Time  (minutes) 


Figure  5.8  EKF  Velocity  Tracking,  q * 0 
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Range  Tracking  Performance  (Error  in  nautical  miles  with  1 sigma  bounds) 


Figure  5.9  MGEKF  Range  Tracking,  q * 0 
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Actual  and  Estimated  Closing  Velocities 


Figure  5. 10  MGEKF  Velocity  Tracking,  q * 0 
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Figure  5. 1 1 EKF  Range  Tracking,  + 2 a 
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Time  (Minutes) 


Figure  5. 12  EKF  Range  Tracking,  - 2a 
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Time  (Minutes) 


Figure  5. 13  MGEKF  Range  Tracking,  + 2 o 
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Range  Tracking  Performance  (Error  in  nautical  miles  with  1 sigma  bounds) 


Figure  5. 14  MGEKF  Range  Tracking,  - 2a 

It  has  been  demonstrated  that  the  EKF  (and  MGEKF)  exhibits  a bias  in  its  range  and 
range-rate  estimates  but  the  cause  has  not  been  addressed.  It  was  conjectured  that  the  cause 
of  the  bias  is  a correlation  between  the  gain  and  innovations  sequences  due  to  the  stochastic 
nature  of  the  linearization.  This  is  consistent  with  the  observation  that  the  IEKF  (which  has 
a deterministic  gain  sequence)  does  not  exhibit  bias.  To  test  this  conjecture  a simulation 
experiment  was  performed.  The  gain  sequences  for  each  of  the  1 00  runs  of  the  Monte  Carlo 
set  (with  q = 0,  results  shown  in  figure  5.1)  was  saved,  averaged  across  the  set  and  stored 
in  a file.  The  EKF  was  then  modified  to  read  in  those  values  and 
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use  them  instead  of  computing  them  on-line.  Thus  instead  of  the  results  determining, 

E[KCm-hCm 

the  results  give 

E[K(x)]E[(z-h(i))] 

Thus  the  result  will  reveal  whether  there  is  a correlation  and  if  it  is  the  cause  of  the  bias. 
The  resulting  range  tracking  plot  (figure  5.15,  next  page)  shows  that  the  bias,  evident  in 
figure  5.1,  has  disappeared  and  the  conjecture  is  thus  validated. 

A similar  experiment  with  the  MGEKF  yielded  the  same  result.  The  empirical  dis- 
covery that  a correlation  between  the  gains  and  innovations  sequences  has  now  motivated 
the  mathematical  investigation  detailed  in  the  next  section. 


Error  (nmi) 
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Figure  5.15  EKF  Range  Tracking,  Stored  Gains 


SECTION  6 


MATHEMATICAL  ANALYSIS  OF  THE  BIAS 

The  previous  section  has  provided  the  motivation  for  the  following  analysis.  The  fact 
that  eliminating  the  correlation  between  the  gain  and  innovations  sequences  resulted  in 
removal  of  the  bias  can  not  be  of  any  practical  interest  unless  a realizable  method  of  removing 
the  correlation  (the  cause)  or  the  bias  (the  effect)  can  be  found.  In  this  section  an  analysis 
will  be  made  to  obtain  an  estimate  of  the  bias.  No  previous  (successful)  effort  has  made  an 
analysis  of  the  global  properties  of  the  estimates  of  the  EKF.  This  is  not  unexpected  since 
the  dependence  of  the  gain  and  covariance  matrix  sequences  on  the  estimated  trajectory 
(and  thus  on  all  previous  measurement  noise  samples)  leads  to  extremely  complicated 
expressions  for  the  conditional  probabilities  after  only  a few  iterations  and  thus  no  general 
conclusions  can  be  drawn. 

The  analysis  presented  here  is  motivated  by  the  realization,  due  to  the  results  in  the 
previous  section,  that  a bias  exists  and  that  it  is  caused  by  a correlation  between  the  gain 
and  innovations  sequences.  This  analysis  would  still  be  impossible  if  the  entire  state  update 
term  was  studied  as  written.  The  clarifying  insight  is  to  consider  the  a priori  state  estimate 
x.k,k-i  as  a perturbation  of  the  actual  state.  Results  using  the  IEKF  lead  to  the  hypothesis 
that  if  the  linearization  is  evaluated  at  the  actual  state  the  bias  noted  in  the  EKF  does  not 
arise,  a conjecture  validated  via  simulation.  Since  this  perturbation  is  stochastic  in  nature 
the  following  analysis  is  termed  a "stochastic  perturbation  analysis"  [42]  and  as  with  any 
perturbation  analysis  only  the  first  order  terms  in  the  Taylor  series  expansions  are  considered. 
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The  EKF  gain  can  be  written  as  (M  is  now  used  to  represent  Pk  k_k) 


K&  = MHT(x)[H(x)MHT(i)  + R]1  (6.1) 

where 


H&)  = 


dh(x) 

dx 


Jc 


0 0]r~2 


(6.2) 


K(x)  and  H(x)  have  been  written  explicitly  as  functions  of  the  a priori  state  estimate  x_  to 
emphasize  this  fact.  Then  defining 

is:(x)  = ^(x)  + 8/s:(x)&  (6.3) 

Ht(x)  « H\x)  + 8Ht(x)8x  (6.4) 


where 


and 


8K(x)  = 


dK(x) 

dx 


8Ht(x)  = 


dffT(x) 

dx 


d2h(x) 

dx2 


(6.5) 

(6.6) 


8x=x-x  (6.7) 

Observe  that  8H(x)  is  the  Hessian  of  the  measurement  function  and  is  thus  symmetric  and 
the  transpose  can  be  neglected. 

Remark : In  the  following  we  will  use  the  convention  of  placing  numerical  scalars  (such 
as  "2"),  when  they  occur,  to  the  extreme  left  of  all  expressions  and  non-numerical  scalars 
(such  as  "r")  to  the  extreme  right  Partitioned  matrices  will  be  used  for  convenience  only. 
To  verify  the  indicated  matrix  multiplications  the  full  matrices  should  be  written  out, 
multiplied  and  then  repartitioned  (although  some  direct  partitioned  multiplication  can  be 
done). 
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Applying  the  chain  rule  to  find  (6.5)  yields 

8£(£)  = - 2MHT(x)H(x)MbH(x)  [H{x)MHT{x)  + R\2 

+ MSH(x)[H(x)MHt(x)  + R]1  (6.8) 

Suppressing  the  (x ) and  letting  D = HMHT  + R (this  is  a scalar)  this  can  be  written  as 


8K  = MbHD~ 1 - 2MHtHM$HD  ~ 2 
= M§HD~l  - 2KHMWD~X 

= (/  - 2KH)MbHD~l  (6.9) 

The  a priori  estimate  of  the  measurement,  z=h(x),  which  appears  in  the  measurement 

residual  can  be  written  as  the  sum  of  the  actual  LOS,  0,  and  the  a priori  angular  estimation 
error,  80 

0 = /i(x)  + 80  = 0 + 50  (6.10) 

Using  (6.3)  and  (6.9)  in  the  state  update  term  yields  the  approximation 

Ax=L,k-L,k-i=K(z-h(x)) 

= [K(x)  + SK(x)Sx}[h(x)  + v-h(x)-dQ] 

= [K(x)  + &:(£)&]  [v  - 80]  (6. 1 1 ) 

It  is  obvious  that 


£[£(x)v]  = 0 
and 

£’[8A'(x)S^v]  = 0 

since  v is  zero  mean  and  statistically  independent  of  the  state  and  all  past  measurement  noise 
samples. 

Considering  80  it  is  understood  that  it  is  not  necessarily  zero  mean  but  that  assumption 
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will  be  made  here.  If  it  is  not  zero  mean  then  it  could  be  written  as  the  sum  of  its  mean  and 
a new  zero  mean  component  and  then  those  two  terms  could  be  evaluated  separately.  There 
would  then  be  an  additional  term  involving  the  mean  of  50  but  the  following  analysis  would 
still  apply.  The  assumption  that  50  is  zero  mean  can  be  shown  to  be  quite  reasonable  using 
a simple  simulation.  It  is  also  true  that  5 K(x)  is  a function  of  the  past  measurement  noise 
sequence  (as  is  50)  since  the  covariance  matrix  M is  a function  of  the  a priori  estimation 
sequence  via  the  linearization  used  in  the  covariance  update  equation  (3.38*?  for  the  EKF 
and  3.44  for  the  MGEKF).  Thus  it  can  not  be  said  that  50  and  5A"(x)  are  uncorrelated  but 
this  is  evaluated  to  be  a higher  order  effect  and  it  will  be  ignored.  In  section  7 it  will  be 
found  that  this  is  not  actually  true  and  empirical  observations  will  be  made  on  the  effect  of 
using  the  stochastic  M.  Thus  it  is  assumed  that 

E[K(xm~0  (6.12) 

and  then  the  only  important  part  of  Ax  is 

Ax=-8K(x)8x8Q  (6.13) 

Since  Sx  and  50  are  both  functions  of  the  past  measurement  noise  sequence  it  is  suspected 
that  the  expected  value  of  their  product  is  not  necessarily  zero. 

To  investigate  the  full  term  is  written  out. 

Ax  = - (7  - 2KH)M8HD~  !Six50  (6. 14) 

Consider  just  the  5/7  D“  'Six 50  term  for  the  moment  and  write  out  5/7, 


577 (x) = 


0 

0 


-(* 2-y2)  0 

-2xy  0 

0 0 

0 0 


0 

0 

0 

0 


r 


-4 


This  can  be  factored  as 
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5 H(x)  = 


yr  2 xr  2 


-xr 

0 

0 


-2 


yr 

0 

0 


xr  2 yr  2 00 

yr~2  -xr~2  0 0 


Using  (6.2)  for  H(x)  and  the  unit  vector  G(x)  orthogonal  to  H(x), 

G(x)  = [x  y 0 0]r_1 

this  can  be  written  in  partitioned  form  as 


(6.15) 


5 H(x)  = [-Ht  GV1] 


-r 


Gr 
-H 


Note  that  G(x)  is  the  gradient  of  the  function  r = {x2  + y2)m  and  thus  is  in  the  direction  of 

increasing  range.  The  a priori  state  estimation  error  can  be  written  as  (appendix  B) 

Sue  ~ HTr28Q  + GTSr 


= [HTr 2 GT] 


50- 

8r 


Then 


5//Z)"15x50  = [-//r  GV] 


-r 


Gr 
-H 


l HTr 2 Gt] 


502 
Sr  50 


D~l 


\-Ht  GV1] 

= [-GV‘  -HTr~l] 
lGT 


' 0 

-n 

r 

' 502  ‘ 

-1 

0 _ 

Sr  50 

D 


-i 


802 
Sr  50 


D 


-i 


r\-  1 — 1 

D r 


Reintroducing  the  -(I- 2 KH)M  gives 


48 


Ax~(I-  2KH)M[Gt  Ht ] 


502 
8r  80 


D~lr~l 


(6.16) 


This  is  a first  order  approximation  of  the  bias  in  cartesian  coordinates.  Since  the  bias  has 
been  observed  to  be  in  the  range  (and  range-rate)  estimate  this  can  be  projected  into  polar 
coordinates  by  premultiplying  by  the  appropriate  gradient  vector,  in  this  case  G for  range 
(the  range-rate  will  be  considered  later).  Thus  an  approximation  for  the  range  update  term 
is 


Ar  = ?*.k  ~ ~ GAx  « G(I  - 2KH)M[Gt  Ht ] 


= GM[G  Ht ] 


= [ GMGt  GMHt ] 


' 502 
8r80 

502 

8r80 


D~lr~l  -2GKHM[Gt  Ht ] 


502 

8r80 

' 802 
hr  56 


D-  1 — 1 

r 


D~lr~l 


D~lr~l  -2GK[HMGt  HMHt ] 


802 

8rS0 


D~lr~l 


Defining  some  terms  (details  in  appendix  B), 


GMH  = o, 


/-e 


GMGT  = ot 


GK  = GMHTD~1  = or(p-1 


yields 


502 

8r80 


D-  1 — 1 

r 


MS  -23Urfa  ?J) 

= (a^502  + clehrho]D~  lr~ 1 - 2a^^a^e802  + o^8r  80)d~V  1 

= (5  - 2or(pr(p-l)w2D-lr- 1 + ?e(l  - 2^D~ ')8r80D-  V- 1 
Taking  the  expected  value  considering  only  8r  and  80  as  random  variables  gives 
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E[Ar]  -[(5-25,?9D-|)o2+(l 

This  can  be  manipulated  into  the  following  form  by  noting  that  D = ol  + R and  expanding 


and  regrouping  terms, 

E[Ar]  = [(*?  + (o^ - + [r  -'$$«<&] D~2r~l  (6.17) 

This  term  appears  in  [43]  and  section  8 for  the  range  estimation  bias.  Evaluating  this  term 
some  strong  statements  can  be  made  about  its  sign.  The  first  is  that  R is  definitely  positive 
(obviously  is  positive).  The  next  term  is  not  as  straightforward  but  with  a little  thought 
it  is  concluded  that 

a^-2o^CTr2e>0 

if  the  correlation  coefficient  of  50  and  8r  is  less  than  .7071  in  magnitude  (a  very  significant 

correlation).  This  degree  of  correlation  has  never  been  observed  even  along  trajectories 
with  very  high  LOS  rates  where  it  is  largest  (it  approaches  zero  for  a zero  LOS  rate).  If  the 
reasonable  assumptions  that  R > oj  and  that  o^e  and  o?e  generally  have  the  same  sign  (even 
if  they  are  nonzero)  then  the  rest  of  the  above  term  is  also  positive  and  it  is  concluded  that 

£[Ar]>0 

which  is  a definition  of  a biased  estimate.  The  sign  of  this  bias  coincides  with  the  direction 
of  the  observed  bias  thus  adding  credence  to  this  analysis.  As  stated  before  this  analysis 
can  not  be  taken  as  proof  that  a bias  exists  but  rather  is  a mathematical  justification  for  the 
bias  observed  in  simulation  studies  in  section  6 and  as  a first  order  estimate  of  the  bias  used 
to  produce  an  estimator  with  reduced  bias  in  section  8.  The  predictive  power  of  this  term 
will  be  investigated  via  simulation  in  the  next  section. 

Applying  the  same  method  to  determine  the  range-rate  update  term  gives 
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Ar  = JAx  = 7(7  - 2KH)M[GT  HT] 


802 

8r  56 


D~xr~l 


where  J is  the  unit  vector  in  the  direction  of  the  range-rate, 

7 = [0  0 x y]r_1 

Repeating  the  same  steps  as  above  and  noting  that 


JMGT  = o*rf 


JK  = JMHTD~l  = a1f9D~1 


this  can  be  written  as 


(6.18) 


It  can  be  reasonably  argued  that  this  expression  is  also  invariably  positive.  Ignoring 
the  obviously  positive  factors  the  terms  o^,  - 2a^eOye  and  o*,a^  are  identified  as 

possibly  negative.  It  has  been  observed  via  simulation  that  these  terms  are  consistently 
positive  over  a wide  range  of  conditions  but  reasonable  qualitative  arguments  can  also  be 
made  for  this  conclusion.  For  example,  consider 


?,-c £ = £[&■&•] 


If  it  is  assumed  that  8r  > 0 and  that  this  error  is  integrated  in  the  state  propagation  equation 
then  it  would  be  expected  that  8r  is  more  probably  (though  not  necessarily)  positive.  The 
same  is  true  if  8 r < 0, 8r  would  be  more  probably  negative.  Thus  the  expected  value  of  the 
product  would  be  positive.  Although  it  may  be  that  is  not  a good  estimate  of  the 
difference  will  tend  to  be  in  magnitude,  not  sign.  This  can  be  seen  by  looking  at  the  definitions 


of  these  terms. 
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Similarly  looking  at  the  term  a^eCT^e  it  can  be  concluded  that  since  8r  and  8r  tend  to  have 

the  same  sign  (on  the  average)  these  two  covariances  would  tend  to  have  the  same  sign  and 
therefore  their  product  would  be  positive.  The  last  term  is  not  as  clear  but, 

— 2ar0a/e  > 0 

if  the  correlation  coefficient  of  8r  with  8r  is  greater  than  twice  the  product  of  the  correlation 

coefficients  of  8r  with  80  and  8r  with  80.  This  reasonable  conclusion  is  verified  by  sim- 
ulation results. 

With  the  above  arguments  the  conclusion  is  drawn  that 

E[Af]  > 0 

The  sign  of  this  bias  is  also  consistent  with  that  observed  in  the  simulation. 

In  the  remainder  of  this  report  the  following  notation  will  be  used.  We  define  A r and 

Ar  to  be  the  actual  biases  introduced  into  the  estimates  at  each  update.  These  terms  can 
only  be  determined  in  a simulation  and  then  only  if  other  extraneous  effects  are  not  present 
(as  will  be  done  in  the  next  section). 

From  the  above  analysis  the  predicted  biases  are  defined  as 

& =G(x)[(I -2K(x)H\x)]M(G(x)M2  + HT(x)mr)  (6.19) 

A/  = J(x)  [(/  - 2K(x)Ht(x)]M(G(x)W2 +HT(x)mr)  (6.20) 

These  predicted  values  can  also  only  be  determined  by  simulation  because  they  require 
knowledge  of  the  actual  a priori  angular  error  80  and  use  the  actual  state  in  the  linearization. 
The  estimated  biases  based  only  on  quantities  that  are  available  to  the  filter  are  defined 

Ar  =GCmi -mx)H\i)]M{GC^  + H\i)#rQ)  (6.21) 

Af  =7(x)[(/-2/s:(x)//7'(i)]M(G(£)82e  + //r(£)^6)  (6.22) 
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It  was  postulated  that  not  all  of  the  elements  in  the  above  terms  contribute  to  the  bias. 
Two  particular  hypotheses  were  considered: 

1)  The  stochastic  variation  in  the  denominator  does  not  contribute  to  the  bias  (this 
is  the  "2HK"  part) 

2)  The  a priori  range  estimation  error,  8r,  does  not  contribute  to  the  bias 

In  the  course  of  the  analysis  of  the  next  section  the  full  terms  above  were  compared  with 
those  from  which  the  denominator  terms  were  deleted.  No  difference  was  observed  so 
conjecture  1)  was  validated.  Conjecture  2)  will  be  validated  in  section  8. 

In  the  next  section  the  properties  of  the  actual,  predicted  and  estimated  biases  will  be 
explored  via  simulation. 


SECTION  7 


SIMULATION  ANALYSIS  OF  THE  BIAS 

The  first-order  terms  for  the  bias  derived  in  the  previous  section  need  to  be  compared 
to  actual  simulation  results  in  order  to  determine  how  well  they  predict  the  bias  in  the  EKF 
and  thus  whether  they  can  be  used  to  correct  for  the  bias.  To  do  this  we  start  with  the  most 
basic  possible  test.  For  proper  comparison  all  possible  effects  that  may  reduce  or  aggravate 
the  bias  must  be  removed.  Thus  the  following  conditions  for  the  test: 

1)  Both  the  observation  platform  and  the  target  are  stationary 

2)  The  initial  covariance  on  the  velocity  estimates  are  zero 

3)  There  is  no  process  noise  and  the  process  noise  covariance  matrix  is  set  to  zero 
Condition  1)  makes  the  range  unobservable  so  that  only  the  bias  effect  is  seen.  Condition 
2)  removes  the  effect  of  the  range-rate  bias  so  that  only  the  range  bias  is  seen  (the  range-rate 
bias  will  be  investigated  later).  Condition  3)  keeps  the  velocity  covariance  zero  by  not 
introducing  any  uncertainty  into  the  filter  via  process  noise. 

The  test  was  set  up  with  the  own-ship  and  the  target  1 nmi  apart  with  the  following 
initial  covariance  matrix: 

'.0625  0 0 O' 

0 .0049  0 0 

0_  0 0 0 0 

. 0 0 0 0_ 

The  noise  covariance  is  the  same  as  before  ( R = .0049  representing  a la  of  4 degrees).  The 
filter’s  estimates  are  initialized  using  the  proper  value  for  the  angular  covariance  (cross 
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range)  but  with  the  actual  range.  In  addition  the  a priori  range  estimate  is  always  reset  to 
the  actual  range  while  preserving  the  angular  error.  Thus  what  is  done  is  to  start  with  the 
range  estimate  equal  to  the  actual  range,  allow  the  filter  to  update,  determine  the  error 
introduced  by  the  update  and  return  the  range  estimate  to  the  actual  value.  The  filter  was 
run  for  30  updates  and  a Monte  Carlo  set  of  30,000  runs  was  made  in  order  to  smooth  the 
data  sufficiently.  This  set  of  data  will  be  referred  to  as  Data  Set  #1.  In  the  following  we 
will  refer  to  the  ensemble  averages  taken  across  this  Monte  Carlo  set  as  the  "expected  value" 
of  the  respective  quantities  and  denote  it  as  E[  ■ ]. 

This  very  simple  test  yielded  some  unexpected  results.  The  actual  average  range  bias 
(£[Ar],  solid),  the  prediction  using  the  actual  state  and  errors  (£[ArJ,  dashed)  and  the 
estimate  using  the  filter  values  (£[Ar],  dotted)  are  plotted  together  in  figure  7. 1 (next  page). 
It  is  seen  that  at  the  beginning  all  three  values  are  equal  but  that  the  predicted  value  and  the 
estimate  are  considerably  larger  than  the  actual  bias  at  later  updates  (the  variation  between 
the  prediction  and  the  estimate  is  less  than  4%  and  will  be  ignored  as  insignificant). 

To  make  the  relationship  more  explicit  the  ratio  £[Ar]  /£[Ar]  (solid)  is  plotted  in 
figure  7.2  (page  56).  Here  a very  interesting  trend  is  seen,  the  ratio  appears  to  decrease 
monotonically  with  each  update  and  in  fact  it  appears  that  the  ratio  is  approximately  equal 
to  the  reciprocal  of  the  update  number.  To  see  this  the  ratio  and  this  reciprocal  (*’s  at  each 
update  point)  are  plotted  in  figure  7.3  (page  57).  This  shows  an  almost  exact  relationship, 
particularly  for  the  first  several  iterations,  with  a possible  divergence  appearing  later. 
However  this  occurs  when  the  bias  becomes  very  small  (because  the  a priori  angular  esti- 
mation error  is  declining  and  is  thus  hard  to  see).  It  was  conjectured  that  the  difference 
between  the  estimate  and  the  actual  bias  was  due  to  one  of  two  causes: 

1)  The  neglected  higher-order  terms  in  the  derivation 

2)  The  neglected  correlation  between  the  covariance  matrix  and  the  other  terms  in  the  gain 


Range  Estimation  Error 
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Update  Number 

Figure  7. 1 Range  Errors,  Data  Set  #1 


solid  = £[Ar]  dashed  = £[Ar]  dotted  = £[Ar] 


Ratio:  Actual/Estimate 
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Figure  7.2  Ratio  of  Range  Errors,  Data  Set  #1 


solid  = £ [Ar]  / £[Ar] 
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Update  Number 


Figure  7.3  Ratio  and  Reciprocal  of  Update  Number 


solid  = £[Ar]  / £[Ar]  * = Reciprocal  of  Update  Number 


Possibility  1)  was  investigated  by  attempting  to  include  the  higher-order  terms  in  the  der- 
ivation but  this  was  quickly  recognized  to  be  an  insurmountable  task  beyond  the  second-order 
term.  Possibility  2)  is  more  amenable  to  study  using  a simulation  experiment.  This  was 
done  by  saving  all  the  values  of  the  covariance  matrix  at  each  time  instant  and  for  each  run 
of  the  Monte  Carlo  set.  These  terms  were  then  averaged  across  the  Monte  Carlo  set  to  obtain 
the  "expected  value"  of  the  matrix.  This  matrix  was  saved  and  the  filter  was  modified  to 
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read  it  in  and  use  it  instead  of  its  own  covariance  matrix.  This  then  removes  the  effect  of 
the  correlation  as  was  done  with  the  gain  in  section  5.  Rerunning  the  entire  30,000  run  set 
(Data  Set  #la)  and  plotting  £[Ar]  (solid),  £[Ar]  (dashed)  and  £[Ar]  (dotted)  as  before  a 
notable  change  is  seen  - the  actual  bias,  the  prediction  and  the  estimate  all  agree  quite  well 
(figure  7.4,  next  page).  Note  that  the  prediction  and  the  estimate  are  identical  to  those  in 
the  previous  test  and  it  is  the  actual  bias  that  has  changed  (increased).  Thus  we  conclude 
that  the  effect  of  using  the  stochastic  covariance  matrix  is  to  reduce  the  total  bias  introduced 
into  the  estimate  relative  to  that  predicted  by  the  analysis  in  the  previous  section.  Obviously 
this  needs  to  be  taken  into  account  when  designing  a filter  that  uses  the  bias  estimate  to 
correct  its  state  estimate  (as  will  be  done  in  section  8).  The  other  result  of  this  observation 
is  to  conclude  that  the  higher  order  terms  that  were  dropped  in  the  perturbation  analysis  are 
in  fact  insignificant. 

For  the  next  simulation  experiment  the  initial  velocity  covariances  are  made  nonzero 
so  that  the  range-rate  bias  can  be  observed.  The  same  Monte  Carlo  set  was  run  but  with  the 
initial  covariance  matrix: 

'.0625  0 0 0 ' 

0 .0049  0 0 

0_  0 0 100  0 

. 0 0 0 100 

The  same  procedure  as  with  Data  Set  #1  of  returning  the  range  (and  now  range-rate)  esti- 
mate^) to  their  actual  values  after  each  update  was  used.  This  set  of  data  is  referred  to  as 
Data  Set  #2.  The  results  for  the  range  are  found  in  figure  7.5  (page  60).  This  shows  something 
new  - there  is  now  a divergence  of  £[Ar]  and  E[Ar]  from  one  another  as  well  as  from  the 
actual  bias.  It  appears  that  this  effect  has  a predictable  asymptotic  relationship  which  will 
be  explored  later.  Plotting  the  corresponding  terms  for  the  range-rate  in  figure  7.6  (page 
61)  obtains  a similar  divergence  of  the  actual,  predicted  and  estimated  biases. 


Range  Estimation  Error 
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Figure  7.4  Range  Errors,  Data  Set  #la 


solid  = £[Ar]  dashed  = £[Ar]  dotted  = £[Ar] 


Range  Estimation  Error 
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Figure  7.5  Range  Errors,  Data  Set  #2 


solid  = E[Ar]  dashed  = £[Ar]  dotted  = £[Ar] 
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Figure  7.6  Range  Rate  Errors,  Data  Set  #2 


solid  = £[Ar]  dashed  = E[Ar]  dotted  = £[Ar] 


Based  on  the  observations  above  it  was  conjectured  that  using  the  stochastic  M matrix 
causes  at  least  part  of  the  difference  between  the  actual,  predicted  and  the  estimate  biases 
(both  range  and  range-rate).  This  will  be  investigated  next  but  first  it  is  conjectured  that 
there  must  be  another  effect  taking  place  to  cause  the  divergence  between  A r , A f and  Ar  ,Af. 
To  investigate  this  we  plot  the  ratios  £[Ar]  /£[A^]  (solid)  and  £[A>]  /£[Af]  (dashed)  in 


Ratios  of  Estimates 
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figure  7.7  below. 


Figure  7.7  Ratio  of  Estimates,  Data  Set  #2 
solid  = £[Ar]  /£[Ar]  dashed  = £[Ar]  /£[Ar] 


This  indicates  that  the  same  effect  is  present  in  both  cases.  Recalling  the  form  of  the 
terms  suggests  the  reason. 


Ar=— 5 62 
( oZ+R)r 


(7.1) 
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A r = . 

(o| +R)f 

(7.2) 

Af=  J?"  se2 

(og+*)r 

(7.3) 

A r = 

(o|+*)r 

(7.4) 

Investigating  further  it  was  discovered  that  o?~6?  anda^  = a?,.  However  comparing 

al  = £[662]  and  £[o^|  a difference  is  shown  by  plotting  the  ratio  £[o^]  / ct^  (figure  7.8,  next 
page).  Comparing  this  with  figure  7.7  confirms  that  the  cause  of  the  observed  divergence 
of  the  predicted  and  estimated  biases  is  that  the  filter  overestimates  the  covariance  of  the  a 
priori  angular  estimation  error  and  that  this  effect  appears  to  be  asymptotically  predictable. 

Repeating  the  procedure  used  before  the  expected  value  of  the  a priori  covariance 
matrix  M was  determined  and  used  in  the  same  Monte  Carlo  set.  This  set  of  data  will  be 
referred  to  as  Data  Set  #2a.  The  same  variables  shown  in  figure  7.5  are  replotted  in  figure 
7.9  (page  65)  for  this  data  set.  Once  again  by  removing  the  correlation  due  to  the  stochastic 
M matrix  the  actual  and  predicted  (solid  and  dashed)  biases  agree.  The  dotted  line  is  A r 
and  shows  a difference  that  has  been  explained  by  the  filter’s  overestimation  of  as 
described  above.  Plotting  the  range-rate  terms  yields  similar  results. 

Now  that  the  causes  of  the  differences  between  the  actual,  predicted  and  estimated 
biases  have  been  identified  we  return  to  the  previous  data  set  (#2)  to  determine  a method 
for  correcting  the  initial  estimate  for  use  in  removing  the  bias.  Looking  first  at  the  range 
bias  the  ratio  of  the  filter  estimate  and  the  actual  bias  (£[Ar]  /£[Ar]  (solid),  henceforth 
referred  to  as  the  ’ratio’)  is  plotted  along  with  the  reciprocal  of  the  of  the  update  number 
(as  in  figure  7.3).  In  addition  a particular  factor  that  was  empirically  determined  to 
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Figure  7.8  Ratio  of  Covariances,  Data  Set  #2 


solid  = £[o^]/o^ 
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Figure  7.9  Range  Errors,  Data  Set  #2a 
solid  = E[Ar]  dashed  = £[Ar]  dotted  = E[&r] 

asymptotically  predict  the  relationship  between  the  actual  and  estimated  biases  is  included. 
This  factor  is  equal  to  twice  the  inner  product  of  the  measurement  gradient  and  the  Kalman 
gain, 

2 HK  = 2HMHt(HMHt  + R)~'= 

c il+R 
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This  factor  (+’s)  is  shown  with  the  others  in  figure  7.10  below. 


Figure  7. 10  Range  Error  Ratios,  Data  Set  #2 

solid  = £[Ar]  / £[Ar]  * = reciprocal  of  update  number  + = E[2HK ] 

Observe  that  during  the  initial  transient  period  (first  10  updates)  the  ratio  corresponds  (as 
in  figure  7.3)  with  the  reciprocal  of  the  update  number  but  later  appears  to  coincide  more 
closely  with  this  newly  defined  factor. 

Now  the  ratio  of  the  actual  and  estimated  range-rate  biases  (E[Ar ] / E\Ar\,  solid)  are 
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plotted  and  a slightly  different  factor:  2^2  HK  (+’s)  what  also  shows  an  apparent  asymptotic 

relationship.  The  plot  (figure  7.11  below)  begins  at  the  second  update  since  all  the  range-rate 
terms  are  zero  at  the  first  update. 


Figure  7.11  Range  Rate  Error  Ratios,  Data  Set  #2 


solid  = E[ Ar]  / £[A>]  + = 2^j2E[HK] 


The  factor  2^2  H K seems  to  reliably  predict  the  ratio  of  the  actual  to  the  estimated  bias  after 
the  initial  transient  period.  Note  that  this  transient  period  corresponds  to  the  interval  where 
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the  filter  goes  from  correctly  estimating  the  a priori  angular  error  covariance  to  consistently 
overestimating  it  by  a factor  of  2. 

Using  the  above  empirically  identified  factors  for  the  range  and  range-rate  biases 
corrected  versions  of  the  bias  estimates  are  proposed.  For  the  transient  period  consider. 


A-(0-l  gg 
nti+Ry 


(7.5) 


where  j is  the  update  number.  After  the  transient  period  use, 

^,(2)  _ 2qg  6?6g 
(6£+R)(6%  + R)r 


(7.6) 


For  the  range-rate  use  the  term, 

(c>l  + R)(G1e  + R)f  '7) 

is  used.  Now  the  Monte  Carlo  simulations  are  rerun  (call  this  Data  Set  #3)  to  determine 
how  well  these  new  ’corrected’  estimates  predict  the  biases  and  in  particular  whether  there 
is  yet  another  correlation  introduced  by  the  new  factors  since  once  again  two  stochastic 
variables  are  being  multiplied  (though  Ar(1)  is  calculated  deterministically  from  A r so  no 
correlation  is  involved). 

To  look  at  the  range  bias  £[Ar]  (solid),  E[Ar(iy]  (*’s)  and  E[Ar(2)]  (+’s)  are  plotted  in 

figure  7.12  (next  page).  This  shows  that  Ar(1)  gives  an  excellent  estimate  of  A r at  the 
beginning  but  that  after  the  transient  period  Ar(2)  does  better  asymptotically. 

To  answer  the  question  of  a correlation  between  the  two  factors  of  Ar(2)  (i.e.  A r and 

2HK)  E[Ar(2y]  (solid)  and  E[Ar]E[2HK]  (*’s)  are  plotted  in  figure  7.13  (page  70)  to  see  if 
there  is  a difference.  This  shows  that  there  is  no  difference. 

Looking  at  the  range-rate  bias  it  is  observed  that  the  correlation  between  the  two  factors 
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Figure  7.12  Range  Errors,  Data  Set  #3 


solid  = E[Ar]  * = £[Ar(1)]  + = £[Ar(2)] 
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Figure  7.13  Range  Error  Estimates,  Data  Set  #3 


solid  = E[Ar(2y]  * = £[Ar]  E[2HK] 


can  not  be  ignored.  The  actual  bias  (solid)  and  the  new  corrected  estimate,  A rm  (dashed) 

diverge  as  shown  in  figure  7.14  (next  page).  The  expected  values  of  the  two  parts  of 
Ar(1)  = l^lHKAr  were  determined,  multiplied  and  plotted  (*’s).  Thus  while  the  product 
of  the  expected  values  (which  is  unrealizable  in  practice)  gives  an  apparently  good 
asymptotic  estimate  the  expected  value  of  the  product  (which  is  realizable)  does  not.  Once 
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Figure  7. 14  Range  Rate  Error  Estimates,  Data  Set  #3 


solid  = E[Ar]  dashed  = Ar (1)  *=E  [2yf2HK]  E [A r] 

again  a correcting  factor  to  A r (1)  is  needed  to  reliably  predict  the  bias  on  line. 

Plotting  the  ratio  £[Ar]  / £[Arn)]  (solid)  in  figure  7.15  (next  page)  indicates  that  the 

correlation  between  the  two  parts  of  Ar(2)  causes  a factor  of  2 difference  at  the  beginning 
which  increases  to  a factor  of  4 (2  because  of  the  correlation  and  2 because  of  the  overes- 
timation of  CJ§). 
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Figure  7.15  Range  Rate  Error  Ratio,  Data  Set  #3 


solid  = £[Ar]  I E[&r(iy\ 


Having  determined  a number  of  candidate  forms  for  the  bias  estimates  it  is  now 
necessary  to  confirm  their  asymptotic  behavior  by  running  a longer  simulation  and  observing 
their  long  term  predictive  performance.  Using  the  same  form  of  the  simulation  the  length 
was  extended  to  300  updates  the  size  of  the  Monte  Carlo  set  was  decreased  to  20000.  This 
will  be  called  Data  Set  #4. 
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The  terms  £[Ar]  (solid),  E[Ar ] (dashed),  £[A>(1)]  (*’s)  and  £[Ar(2)]  (+’s)  are  plotted 
for  updates  10  to  50  (i.e.,  after  the  initial  transient  period)  in  figure  7.16  below. 


xlO-3 


Figure  7.16  Range  Errors,  Data  Set  #4  (points  10  to  50) 
solid  = £[Ar]  dashed  =£[Ar]  * = £[Ar(1>]  + = £[Ar(2)] 

As  expected  the  original  estimate  A r greatly  overestimates  the  bias  and  Ar(1)  has  become 

poor  after  the  initial  transient  period  where  it  had  been  best.  It  is  apparent  that  A r(2)  has 
become  the  best  estimate  during  this  period  but  it  seems  that  by  update  50  it  is  poor  as  well. 
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Plotting  points  50  to  300  of  E[Ar]  (solid),  £[Ar]  (dashed),  and  ^[Ar^  (+’s)  in  figure 

7.17  below  shows  that  in  the  long  run  E [Ar(2)]  (+’s)  does  not  give  a consistently  good  estimate 
but  rather  that  A r (dashed)  seems  to  track  £[Ar]  (solid)  asymptotically  to  within  a constant 
multiplier. 
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Figure  7.17  Range  Errors,  Data  Set  #4  (points  50  to  300) 
solid  = £[Ar]  dashed  = £[Ar]  + = £[Ar(2)] 


Taking  the  average  of  A r over  updates  50  to  300  and  dividing  by  the  average  of  E[Ar] 


Range  Bias 
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over  the  same  period  yields  a ratio  of  4. 1689.  Thus  an  approximate  asymptotic  relationship 
can  be  defined, 

£[Ar]  /4.1689  «.E[Ar]  (7.8) 

Plotting  £[Ar ] (solid)  and  ZsIAr]  / 4.1689  (dashed)  in  figure  7.18  (below)  indicates  a long 

term  asymptotic  relationship  which  will  be  exploited  in  the  next  section  in  designing  the 
first  estimator. 


xlO-3 


Figure  7.18  Range  Error  and  New  Estimate,  Data  Set  #4  (points  50  to  300) 


solid  = E [Ar]  dashed  = E [Ar  ] / 4. 1 689 
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Plotting  the  range-rate  terms  E[ Ar]  (solid),  E[Ar]  (dashed)  and  £[Af(1)]  (*’s)  for 

updates  10  to  50  in  figure  7. 19  (below)  shows  that  Air (1)  turns  out  to  not  be  a good  long  term 
estimate  of  E[Ar ] but  that  both  E[Ar ] and  ZifA/^1*]  exhibit  a constant  asymptotic  relationship 
with£[Ar]. 


) 1 1 L I _l | | 

10  15  20  25  30  35  40  45  50 


Update  Number 

Figure  7.19  Range  Rate  Errors,  Data  Set  #4  (points  10  to  50) 
solid  = £[Ar]  dashed  = E[&r]  * = £[A^(1)] 


Choosing  to  use  £[Ar]  (it  is  the  simpler  of  the  two)  and  taking  its  average  over  updates  50 


Range  Rate  Bias 
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to  300  and  dividing  by  the  average  of  £[Ar]  over  the  same  period  yields  a ratio  of  3.9597. 
So  the  asymptotic  relation, 

£[Ar]  = £[Ar]  / 3.9597  (7.9) 

is  determined  to  be  empirically  valid  as  is  apparent  from  figure  7.20  below. 


Figure  7.20  Range  Rate  Error  and  New  Estimate,  Data  Set  #4  (points  50  to  300) 
solid  = £[Ar]  dashed  = £[Ar]  / 3.9597 


Though  terms  that  exhibit  excellent  performance  in  estimating  the  range  and  range-rate 


78 


biases  have  been  identified  a number  of  questions  remain.  Among  these  are:  Will  these 
estimates  perform  under  a wide  range  of  conditions?  In  particular,  will  they  perform  in  a 
dynamic  scenario  (these  results  are  from  static  tests)?  Also,  will  they  perform  over  an  even 
longer  period  of  time  than  has  been  tested  here?  These  questions  will  be  answered  in  section 
9 using  an  estimator  derived  using  the  results  of  this  section. 

As  an  epilogue  to  the  previous  analysis  we  present  here  the  results  of  a similar 
investigation  into  the  bias  properties  of  the  MGEKF.  Because  it  was  found  that  the  stochastic 
nature  of  M plays  a role  in  the  bias  and  the  MGEKF  calculates  it  in  a different  manner  than 
the  EKF  different  results  should  be  expected.  Using  the  same  set  up  as  with  the  previous 
analysis  and  using  nonzero  velocity  terms  the  plot  for  £[Ar]  (solid),  £[Ar]  (dashed)  and 
£[Ar]  (dotted)  is  given  in  figure  7.21  (next  page,  compare  with  figure  7.5). 

This  shows  the  same  trend  as  with  the  EKF  but  it  appears  that  the  MGEKF  range  bias 
becomes  even  more  negligible  than  the  EKF’s  by  about  the  5th  update  and  perhaps  even 
become  negative  afterward. 

Looking  at£[Ar]  (solid),  £[AF]  (dashed)  and  E[&r]  (dotted)  in  figure  7.22  (page  80), 

compare  with  figure  7.6)  it  is  seen  that  this  is  again  different  than  the  EKF  and  the  actual 
bias  may  become  negligible  towards  the  end.  An  important  observation  to  make  is  that  the 
actual  biases  do  not  seem  to  be  asymptotically  predictable  from  their  estimates  as  with  the 
EKF.  For  this  reason  a corrected  version  of  the  MGEKF  was  not  pursued. 


Range  Estimation  Error 
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Figure  7.21  MGEKF  Range  Errors 


solid  = £[Ar]  dashed  = £[Ar]  dotted  = £[Ar] 


Range  Rate  Estimation  Error 
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Figure  7.22  MGEKF  Range  Rate  Errors 
solid  = £[Ar]  dashed  = £[Ar]  dotted  = £[Ar] 


SECTION  8 


FIRST  FILTER  SYNTHESIS 


Based  on  the  mathematical  and  simulation  analysis  of  the  previous  sections  the  first 
of  three  possible  solutions  to  the  problem  of  bias  in  the  EKF  as  applied  to  the  passive  tracking 
problem  is  proposed.  In  this  section  the  first  order  estimate  of  the  bias  derived  in  section  6 
is  modified  based  on  empirical  observations  made  in  section  7 and  subtracted  from  the  a 
posteriori  estimate  obtained  from  the  standard  EKF.  This  "correction"  is  done  at  each 
measurement  update  leading  to  an  estimate  with  much  less  bias  than  the  standard  filter.  The 
bias  estimate  is  adjusted  to  compensate  for  the  effects  of  the  M-matrix  correlation  identified 
in  section  7.  Because  these  modifications  are  based  on  observation  and  not  analytic  deri- 
vation the  theoretical  support  for  this  filter  is  not  complete.  However,  the  simulation  results 
shown  in  the  next  section  will  illustrate  the  effectiveness  of  this  approach  and  encourages 
further  work  along  this  line  of  investigation. 

In  section  6 the  first-order  estimates  of  the  range  and  range-rate  biases  were  derived, 

(8.1) 


A r = 
A?  = 


(Raj  + (ajoe  “ 25e?e))S02  + {r  ~ Oe)<?e8r80]£)  2r  ‘ 
+ (38  - 23e3e)>02 + [r  - 3)?>se]D-: V-1 


(8.2) 


These  contain  all  of  the  terms  derived  in  the  previous  analysis.  It  was  conjectured  (motivated 


by  an  initial  failure  of  this  method)  that  only  certain  terms  in  these  estimates  have  a significant 
effect  on  the  bias  and  that  the  other  terms  should  be  dropped  for  simplicity  and  to  avoid 
introducing  excess  noise  into  the  filter  (these  terms  are  quite  noisy).  The  first  question  to 
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be  answered  is  whether  the  effect  of  the  stochastic  linearization  H(xJck_l)  is  greatest  due  to 
its  presence  in  the  "numerator"  ( MH' ) or  in  the  "denominator"  ( MHAf)  of  the  Kalman  gain 
matrix  K.  This  effect  was  investigated  in  the  last  section  by  calculating  both  the  full  terms 
(8. 1,8. 2)  and  the  terms  with  the  denominator  factors  removed, 


Ar  = (/?  + g^)o^802  + (/?  --a^)a^e5/-50]z)  2r  1 

(/?  + a^)a^502  + (y?  -^^e8/-50]D_2r_1 


A f = 


(8.3) 

(8.4) 


The  second  question  is  whether  the  a priori  range  or  angle  estimation  error  is  the 
dominant  bias  source.  It  was  assumed  in  section  7 that  it  is  the  angle  error  that  causes  the 
error  and  the  range  error  is  not  a factor.  This  assumption  will  be  validated  here.  For  this 
experiment  the  EKF  and  IEKF  linearizations  are  written  in  polar  coordinates, 


EKF  Lineariza- 
tion: 

IEKF  Lineariza- 
tion: 

and  two  filters  are  postulated  with  linearizations. 
Filter  A: 


H(x)  = 


H(x)  = 


sin(0)  cos(0) 


sin(0)  cos(0) 


00 


00 


H = 


sin(0)  cos(0) 


00 


Filter  B: 


sin(Q)  cos(Q)  Q Q 
r f 


Filter  A will  exhibit  the  effect  of  the  angle  perturbation  only  and  Filter  B that  of  the  range 
perturbation  only.  These  two  filters  were  tested  in  the  same  scenario  as  in  section  5.  The 
range  estimation  error  plot  for  filter  A is  shown  in  figure  8. 1 (next  page)  and  for  filter  B in 
figure  8.2  (page  84). 
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Figure  8.2  The  Effect  of  the  a priori  Range  Error  Only 


This  shows  that  filter  A performs  almost  identically  to  the  EKF  and  filter  B similarly  to  the 
IEKF.  Thus  it  has  been  verified  that  it  is  the  angle  error  that  causes  the  bias  so  the  term 
involving  the  range  error  is  dropped. 

Using  this  result  the  original  bias  estimates  (6.21)  and  (6.22)  are  reduced  to  very  simple 
forms. 


A-  # ^2 

A r =— o2 

(o| +R)f 


(8.5) 
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r.  & 12 

*“(0 !+*)^ 

(8.6) 

In  matrix  notation  the  estimates  can  be  written  as. 

A r = GMGTHMHT(HMHT  + R)~1r1 

(8.7) 

Af  = JMG tHMHt{HMHt  + R)~  r" 1 

(8.8) 

or  in  cartesian  coordinates, 

Ax  = (GTG+JTJ)MGTHMHT(HMHT  + R)~1rl 

= (GTG+JTJ)MGTHKr~l 

(8.9) 

This  term  (once  modified  based  on  the  results  in  section  7 as  will  be  explained  next)  is 
subtracted  from  the  a posteriori  estimate  at  each  measurement  update. 

The  simulation  experiments  carried  out  in  section  7 indicate  that  due  to  the  stochastic 
nature  of  the  covariance  matrix  M used  in  the  EKF  the  above  estimates  are  not  universally 
valid  and  must  be  modified.  Since  the  range  and  range-rate  biases  behave  somewhat  dif- 
ferently they  will  be  treated  separately.  The  dominant  effect  in  the  range  bias  estimate,  as 
shown  in  figures  7.1  and  7.3,  is  that  the  estimate  is  in  general  greater  than  the  actual  by  a 
factor  approximately  equal  to  the  update  number  at  the  first  several  updates  and  then 
asymptotically  approaches  a factor  of  approximately  four  (see  figures  7.12  and  7. 18).  The 
correction  used  will  be  to  divide  A r by  one  at  the  first  update,  two  at  the  second  and  three 
at  the  third.  After  that  we  divide  by  four  at  all  subsequent  updates.  The  integer  4 was  chosen 
because  it  appears  that  the  asymptotic  relationship  shown  in  section  7 was  approaching  this 
value  and  the  accuracy  of  the  empirical  determined  factor  (4.1689)  is  not  justified  suffi- 
ciently. 

The  range-rate  bias  is  the  more  significant  factor  when  the  filter  is  run  for  a long  time 
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since  this  error  is  integrated  to  produce  a range  bias.  However,  this  bias  and  its  estimates 
are  initially  zero  since  they  is  proportional  to  c£,  (<%,)  which  is  set  equal  to  zero  initially. 
As  with  the  range  bias  estimate  Air  tends  to  overestimate  the  actual  bias  by  a factor  of 
approximately  four  and  thus  it  is  divided  by  four  and  subtracted  from  the  a posteriori  estimate 
at  all  updates  (see  figure  7.20).  Again  the  integer  4 rather  than  3.9597  is  chosen  so  as  not 
to  implicitly  overstate  the  accuracy  of  this  factor. 

To  summarize  the  filter  design  all  of  the  estimator  equations  are  the  same  as  the  EKF 
except  for  the  state  update  equation  which  is  as  follows.  For  updates  j = 1,2,3  the  equation 
is, 

£ m = £ M - 1 + Kk(zk -h(xkk_l))-(GlGk/j  + JTk  Jk / 4 )Mk Gk  Hk Kkf~ 1 (8.10) 

for  j > 4, 

£*.*  = £m-i  + Kk(zk  ~ h (£m_,))  - (Gk  Gk/4  + JTk  Jk/4)M  Gk  Hk Kkr~ 1 (8. 1 1) 

For  lack  of  a better  title  the  above  filter  is  termed  the  "Moorman-Bullock  Extended  Kalman 
Filter"  (MBEKF)  and  its  estimation  performance  is  investigated  via  simulation  in  the  next 
section. 

A final  modification  is  required  due  to  the  fact  that  the  above  filter  does  successfully 
remove  most  or  all  of  the  bias  in  the  EKF.  This  modification  is  required  because  improving 
the  filter  performance  also  causes  it  to  be  less  stable  (a  natural  consequence  of  its  improved 
performance).  The  machinism  of  this  instability  is  very  simple  and  is  caused  by  the  system 
nonlinearity.  Consider  the  linearization  of  the  measurement  function, 

= [-y  x 0 0]r“2 

Note  that  the  filter’s  a priori  range  estimate  f appears  in  the  denominator  of  the  linearization. 
Instability  can  occur  if  the  range  estimate  becomes  very  small  causing  the  gain  to  be  very 
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large.  This  typically  occurs  when  the  extrapolated  target  position  is  very  close  to  the  ownship 
and  as  a consequence  its  estimated  angular  position  diverges  substantially  from  its  actual 
position.  These  two  factors  will  cause  a large  state  update  away  from  the  actual  target 
position  manifested  by  a large  "jump"  in  the  range  and  range-rate  estimates.  This  phenomena 
is  not  observed  in  the  EKF  because  its  inherent  bias  tends  to  keep  the  range  estimate  from 
becoming  small.  In  this  filter  and  the  two  filters  proposed  later  this  phenomena  has  been 
infrequendy  observed  (and  only  when  the  filter  is  intentionally  initialized  with  a low  initial 
range  estimate)  so  it  is  necessary  to  add  a modification  to  each  filter  to  avoid  instability. 

The  modification  decided  upon  was  to  test  the  a priori  range  estimate  by  comparing 
it  with  twice  the  filter’s  a priori  range  estimation  error  as  calculated  by  the  filter  itself  (i.e. 
G(xk)MkG(xk)T).  If  the  range  estimate  is  less  the  this  2 a value  it  is  reset  to  that  value  and 
the  target  relative  position  updated  accordingly  (though  the  a priori  LOS  estimate  is 
retained).  This  modification  was  made  to  all  the  filters  so  as  to  keep  the  comparisons  fair 
(results  shown  in  section  5 had  this  modification  to  the  filters). 


SECTION  9 


FIRST  FILTER  SIMULATION  RESULTS 

For  the  purposes  of  comparison  with  the  baseline  filter  (EKF)  and  best  possible  filter 
(IEKF)  performance  the  Moorman-Bullock  Extended  Kalman  Filter  was  run  in  the  identical 
simulation  sets  as  described  in  section  5.  The  range  tracking  performance  plot  using  sto- 
chastic initialization  is  shown  in  figure  9.1  (compare  with  figures  5.1  for  the  EKF,  5.3  for 
the  MGEKF  and  5.5  for  the  IEKF)  and  the  closing  velocity  plot  in  figure  9.2  (compare  with 
figures  5.2  for  the  EKF,  5.4  for  the  MGEKF  and  5.6  for  the  IEKF)  on  the  next  two  pages. 
A considerable  improvement  over  the  EKF  is  evident.  There  is  nearly  no  residual  bias  and 
the  actual  estimation  covariance  is  comparable  with  that  of  the  EKF  (and  the  CRLB)  and 
the  velocity  tracking  displays  much  less  bias.  It  is  important  to  note  that  this  performance 
gain  is  achieved  with  little  impact  on  the  la  of  the  range  estimation  error.  This  is  due  to 
the  fact  that  we  have  trimmed  the  bias  correction  term  down  to  only  its  essential  component 
so  that  little  additional  noise  is  introduced  in  the  process. 

Using  the  nonzero  process  noise  covariance  matrix  demonstrates  the  substantial 
improvement  of  the  MBEKF  over  baseline  filter  as  shown  in  figure  9.3  (page  91,  compare 
with  figures  5.7  for  the  EKF  and  5.9  for  the  MGEKF).  This  shows  a remarkable  improvement 
over  the  EKF.  There  is  nearly  no  residual  bias  though  we  see  that  the  actual  estimation 
covariance  is  now  slightly  greater  than  in  the  EKF  at  most  points  throughout  the  run.  This 
is  due  to  the  fact  that  the  bias  estimation  terms  are  themselves  noisy  and  add  to  the  covariance 
of  the  estimate.  The  range-rate  estimation  performance  (figure  9.4,  page  92)  also  shows  an 
improvement  over  the  EKF. 
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Figure  9. 1 MBEKF  Range  Tracking,  q = 0 
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Figure  9.2  MBEKF  Velocity  Tracking,  q = 0 
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Figure  9.3  MBEKF  Range  Tracking,  q * 0 
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Figure  9.4  MBEKF  Velocity  Tracking,  q ^ 0 


Error  (nmi) 
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The  MBEKF  was  also  run  in  the  same  ± 2a  sets  as  the  EKF  (and  MGEKF).  The  range 

tracking  plots  are  shown  below  and  on  the  next  page.  The  MBEKF  successfully  takes  out 
the  initial  error  and  then  keeps  it  within  its  own  ± la  bounds. 


Figure  9.5  MBEKF  Range  Tracking  + 2 a 


Error  (nmi) 
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Figure  9.6  MBEKF  Range  Tracking  - 2a 


SECTION  10 


SECOND  FILTER  SYNTHESIS 

In  the  previous  two  sections  a filter  based  on  one  approach  to  lessen  or  eliminate  the 
bias  inherent  in  the  cartesian  EKF  was  designed  and  analyzed.  That  particular  approach  is 
straightforward  in  that  it  attempts  to  remove  the  bias  after  it  has  been  introduced.  Another 
approach  is  to  attempt  to  prevent  the  introduction  of  the  bias  in  the  first  place  though  how 
to  go  about  this  is  not  immediately  obvious.  It  was  conjectured  that  if  the  state  estimate 
from  several  sample  times  in  the  past  was  extrapolated  forward  to  the  current  measurement 
update  time  and  used  for  the  linearization  there  would  be  a reduction  in  the  correlation 
between  the  gain  and  innovations  sequences  thus  reducing  or  removing  the  bias.  This  can 
be  expressed  in  the  following  manner, 


where, 


X k,k- 1 ^k-lx. 

(10.1a) 

pk  = + Qk 

(10.16) 

(10.1c) 

(lO.lrf) 

(10.1c) 

OX  *(/) 

£-£* 

(10.2) 
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and 

i'i)  = &kik-j.k-j- 1 (10.3) 

The  use  of  the  predicted  state  x k 0)  in  the  linearization  suggested  that  this  filter  be  called  the 

Predicted  Gain  Extended  Kalman  filter  (PGEKF).  It  should  be  noted  that  this  defines  a 
whole  class  of  estimators  parameterized  by  the  delay  j and  that  the  EKF  is  simply  the  member 
of  this  class  with  j = 0.  Note  also  that  the  "third  term"  in  the  innovations  sequence, 
~Hk*Xx.k,k- l>as  been  reintroduced  since  the  linearizing  state  trajectory  is  not 
identical  to  the  a priori  state  estimate.  The  results  given  in  the  next  section  are  obtained 
with  the  third  term  included  as  written.  Additional  runs  (not  presented  here)  were  made 
with  it  removed  from  the  residual  sequence  and  the  results  indicate  that  without  it  the  PGEKF 
behaves  much  like  the  EKF  though  no  investigation  was  made  as  to  why  this  occurred. 

There  are  still  two  questions  to  be  addressed  now  are:  the  choice  of  the  delay  parameter 
and  how  to  handle  the  start  up  of  the  filter  when  an  estimate  from  j updates  in  the  past  is 
not  available.  The  first  question  was  studied  using  simulation  experiments  since  no  exact 
analysis  can  determine  this.  It  would  seem  obvious  that  if  j was  chosen  to  be  "too  small" 
(say  1 or  2)  that  there  would  be  a trivial  reduction  in  the  bias  (confirmed  by  simulation). 
On  the  other  hand  if  / was  "too  large"  the  tenuous  relationship  between  the  current  state  and 
the  linearizing  estimate  would  limit  filter  performance  (also  confirmed  by  simulation). 
Further  refinements  to  this  concept  will  be  made  in  the  next  section.  As  for  the  second 
question  it  was  decided  to  use  the  following  method  presented  here  as  an  example.  Say 
y=3  was  chosen.  Starting  with  the  initial  estimate  x0  itis  propagated  to  the  first  measurement 
time  (k  = 1)  as 
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and  is  used  for  the  linearization  and  the  estimate  * u is  found  (so  far  this  is  the  same  as  the 


EKF).  At  k = 2, 


= <j>£  (t3)  = 


is  used  rather  than 


1,1 


as  in  the  standard  EKF.  At  k = 3, 


is  used  and  for  k > 4, 


where  xflXk_4  is  the  filter’s  a priori  estimate  from  3 time  steps  (measurement  intervals)  in 
the  past. 

The  filter  as  defined  above  was  implemented  and  tested  with  a number  of  values  for 
the  delay  parameter.  In  summary  it  was  found  that  increasing  the  delay  lessened  the 
introduction  of  the  bias  but  also  prevented  the  filter  from  fully  utilizing  the  information  in 
the  measurement  sequence  as  evidenced  by  excessive  range  covariance  (compared  with  the 
EKF  and  IEKF).  A final  modification  in  the  design  was  implemented  on  the  assertion  (based 
on  previous  observations)  that  the  measurements  taken  during  and  immediately  after  an  own 
ship  turn  are  the  most  informative  and  thus  should  be  processed  as  in  the  EKF  (zero  delay). 
At  other  times  the  measurements  do  not  decrease  the  range  estimation  error  covariance 
significantly  and  a longer  delay  prevents  bias  build  up  without  sacrificing  much  performance. 
To  test  this  hypothesis  the  PGEKF  with  a variable  delay  programmed  in  coordination  with 
the  own  ship  maneuvers  was  executed.  The  particular  scheme  is  presented  in  the  next  section 
along  with  the  simulation  results  showing  promising  performance  using  this  approach. 


SECTION  11 


SECOND  FILTER  SIMULATION  RESULTS 

As  in  section  9 with  the  MBEKF  the  PGEKF  was  run  for  comparison  with  the  EKF 
and  IEKF.  Based  on  a number  of  sets  of  simulation  runs  the  delay  was  programmed  as 
follows.  The  nominal  delay  was  chosen  to  be  20.  During  the  20  updates  before  the  initiation 
of  a turn  the  delay  is  reduced  by  1 so  that  at  the  turn  the  delay  is  0.  During  the  turn  (30 
seconds/updates)  and  for  60  updates  afterwards  the  delay  is  kept  at  0 and  then  increased  by 
1 for  each  of  the  next  20  updates  until  it  is  20  again.  It  remains  20  until  before  the  next  turn 
when  it  is  decreased  again  and  the  cycle  repeats  for  each  own  ship  turn. 

The  simulation  results  are  shown  in  figures  1 1. 1 and  1 1.2  (next  two  pages).  The  bias 
has  been  significantly  reduced  so  that  it  is  trivial  compared  with  the  standard  error  but  shows 
greater  standard  error  than  the  EKF.  Also  the  velocity  tracking  performance  shows 
anomalous  performance  - the  error  is  greater  than  that  seen  with  the  EKF  (figure  5.2)  or 
even  the  MGEKF  (figure  5.4). 

Modeling  the  target  as  maneuvering  by  using  the  nonzero  process  noise  covariance 
matrix  there  is  a vast  improvement  over  the  EKF  and  MGEKF  performance  shown  in  section 
5 and  comparable  to  the  MBEKF  of  section  9 as  indicated  in  figures  1 1.3  and  1 1.4  (pages 
101  and  102). 

The  result  of  running  the  ± 2 a sets  are  shown  in  figures  1 1.5  and  1 1.6  (pages  103  and 

104).  This  indicates  that  the  PGEKF  succeeds  though  perhaps  not  as  well  as  the  MBEKF 
because  of  the  observed  anomaly  in  velocity  tracking  when  q = 0. 
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Figure  11.1  PGEKF  Range  Tracking,  q = 0 
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Figure  1 1.2  PGEKF  Velocity  Tracking,  q = 0 
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Figure  1 1.3  PGEKF  Range  Tracking,  q * 0 
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Figure  1 1.4  PGEKF  Velocity  Tracking,  q * 0 
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Time  (Minutes) 


Figure  1 1.5  PGEKF  Range  Tracking,  + 2 a 
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Figure  1 1.6  PGEKF  Range  Tracking,  -2a 


SECTION  12 


THIRD  FILTER  SYNTHESIS 

As  with  the  previous  design  the  final  estimator  is  based  on  an  effort  to  prevent  the  bias 
from  being  introduced  into  the  a posteriori  estimate  in  the  first  place.  Once  again  the 
approach  is  based  on  a serendipitous  insight.  To  best  illustrate  the  approach  we  consider 
the  functional  depiction  of  the  standard  EKF  shown  in  figure  12.1  (next  page).  The 
mechanism  (dark  line)  by  which  the  a priori  state  estimate  is  fed  back  into  the  estimator  via 
the  linearization  process  has  been  made  explicit.  The  question  is:  how  to  break  this  path? 
This  would  require  some  other  source  for  the  linearizing  trajectory  (and  possibly,  but  not 
necessarily,  a "sink"  for  the  broken  path).  This  does  not  seem  to  be  possible  because  where 
could  such  a trajectory  be  obtained?  Such  a trajectory  can  not  be  pulled  out  of  thin  air 
because  the  filter  must  be  self  contained  to  be  practical.  The  fortuitous  recognition  was  that 
another  filter,  operating  in  parallel,  could  provide  the  estimate  and  in  turn  would  require  its 
partner’s  estimate  for  its  own  linearization.  Thus  we  are  lead  to  the  concept  of  two  identical 
cross-coupled  filters  providing  one  another  with  a linearizing  trajectory.  Of  course  the  two 
filters  could  not  be  fed  the  identical  measurement  data  stream  else  there  would  be  no 
decoupling.  This  final  obstacle  is  removed  if  the  filters  are  fed  two  independent  measurement 
sequences  (this  will  be  discussed  later)  as  shown  in  figure  12.2  (page  107). 

This  filter  can  also  be  described  completely  by  the  following  sets  of  equations, 

Filter  #1 

(12.1a) 
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Figure  12.1  Extended  Kalman  Filter  Functional  Diagram 
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Figure  12.2  Cross-coupled  Federated  EKF  Functional  Diagram 
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where, 


Filter  #2 


where. 


<2, ®L,+a 

(12.1  b) 

Kll)  = M^)Hf)T[H{2)Mf)Hf'  + Rf^ 

(12.1c) 

£K=£2L+W-A(i?. 

(12.1  d) 

(7  - K[{)H{2))M^\l  - K{^H(2))T  + K^R^Kf 

(12.1c) 

(2)  dh{x) 

Hk  ~ dx 

~ i-igL, 

(12.2) 

(12.3a) 

C.+a 

(12.37?) 

Ki2)  = M{2)H^t{h^M^H^)T  + R{2)Y 

(12.3c) 

C (2)  _ C (2)  .jA  2)/_(2)  5(2) 

(12.3  d) 

If  - - Kt2>H«)T  + KfW* 

(12.3c) 

od)  _ dh(*) 

1 ‘“aT 


-CO) 


(12.4) 


Thus  the  linearization  used  in  each  filter  is  based  on  the  estimate  of  the  other.  The  single 
measurement  sequence  zk  with  covariance  Rk  has  been  split  into  two  sequences  zk]  and  zk2) 
with  covariances  and  7?f}  respectively  with 

^=kr,+*r']-  ms) 


Note  that  the  residual  sequences  do  nothave  the  "third  term"  even  though  the  linearizing 
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trajectory  is  not  the  filters’  a priori  estimate.  Simulation  runs  were  made  with  these  terms 
included  and  poor  performance  resulted.  Dropping  the  terms  (which  are 
-Hi  for  filter  #1  and  -Hk\x^2)  kk_^  -x(k\_l)  for  filter  #2)  resulted  in 

obtaining  the  performance  shown  in  the  next  section. 

The  post-processing  "estimate  combination"  is  done  using  simple  linear  estimation 
theory.  The  outputs  {a  posteriori  estimates  and  covariance  matrices)  of  the  two  filters  are 
considered  to  be  independent  estimates  of  the  same  state  vector  (they  actually  are  not)  with 
their  respective  covariances.  The  estimates  will  be  referred  to  as  x{k\  and  xf\  with 
covariances  P(k]l  and  Pk2>k.  The  optimal  linear  combination  in  such  a case  is  given  by, 

n=[pr+/frT  (i2.6) 

L,^ptPl‘r'i  a + />,  p?r  'is  a 2.7) 

This  estimate  and  covariance  are  then  the  final  output  of  the  filter.  A filter  of  this  form  is 
referred  to  as  a "federated"  filter  [44-46]  and  thus  this  estimator  is  called  the  Cross-coupled 
Federated  Extended  Kalman  Filter  (CFEKF). 

There  are  a number  of  questions  that  must  be  addressed  concerning  the  implementation 
of  the  CFEKF.  One  is  the  question  of  obtaining  two  independent  measurement  sequences. 
This  would  seem  to  limit  application  to  situations  where  two  sensors  are  available  to  track 
the  target.  This  is  not,  however,  the  case.  There  are  two  methods  for  accomplishing  this 
with  a single  sensor.  A simple  alternative  is  to  update  each  filter  with  every  other  mea- 
surement (the  non-updated  filter  would  simply  extrapolate  as  usual).  As  long  as  significant 
changes  do  not  occur  between  these  sparser  updates  no  problem  will  be  encountered.  A 
second  approach  can  be  proposed  by  realizing  that  for  most  actual  sensor  systems  mea- 
surements are  not  reported  as  frequently  as  they  could  be.  For  example,  a particular  infrared 
system  may  operate  at  a frame  rate  of  100  Hertz  but  it  may  report  an  angular  position 
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measurement  at  only  a 10  Hertz  rate.  In  this  case  the  image  is  sampled  100  times  a second 
and  10  images  are  averaged  (this  is  referred  to  as  prefiltering  or  postdetection  integration, 
both  very  common  practices)  to  produce  a single  detection/measurement  report.  This  is 
typically  done  to  avoid  calling  the  tracking  filter  routine  at  a higher  rate  than  necessary  so 
as  to  prevent  processor  overload.  The  major  limitation  on  the  integration  time  used  is  that 
the  target  state  must  not  change  appreciably  over  the  interval.  If  the  sensor  does  perform 
such  prefiltering  it  would  be  a simple  matter  to  produce  two  independent  measurements  by 
averaging  half  the  frames  to  obtain  one  measurement  and  the  other  half  for  the  other  mea- 
surement. It  would  be  best  if  this  process  was  interleaved,  i.e.,  as  in  the  example  above 
frames  1,  3,  5,  7 and  9 are  averaged  to  obtain  one  measurement  and  frames  2,  4,  6,  8 and 
10  are  averaged  for  the  second.  This  would  minimize  further  any  possible  corregistration 
errors  in  the  measurements.  In  either  case  this  is  not  an  undue  limitation  for  the  application 
being  proposed. 


SECTION  13 


THIRD  FILTER  SIMULATION  RESULTS 
The  simulation  results  using  the  CFEKF  with  q = 0 are  presented  (next  two  pages). 
In  this  case  there  is  no  improvement  in  performance  and  actually  a decrease— there  is  as 
much  bias  as  in  the  EKF  and  the  la  error  has  increased  so  that  its  performance  looks,  more 
than  any  other  filter,  like  the  MGEKF  (figure  5.3). 

Before  discarding  this  method  the  results  using  the  nonzero  process  noise  covariance 
matrix  shown  in  figures  13.3  and  13.4  (pages  114  and  115)  need  to  be  examined.  These 
show  that  the  CFEKF  does  do  quite  well  under  these  conditions  though  somewhat  less  so 
than  the  MBEKF  and  on  a par  with  the  PGEKF. 

Finally  running  the  ±2a  sets,  figures  13.5  and  13.6  (pages  116  and  117),  show 

comparable  performance  with  the  MBEKF  and  the  PGEKF  indicating  that  the  CFEKF  can 
be  considered  to  be  at  least  a moderate  success. 


Ill 
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Figure  13.1  CFEKF  Range  Tracking,  q = 0 
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Actual  and  Estimated  Closing  Velocities 


Figure  13.2  CFEKF  Velocity  Tracking,  q = 0 
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Figure  13.3  CFEKF  Range  Tracking,  q * 0 
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Time  (minutes) 


Figure  13.4  CFEKF  Velocity  Tracking,  q * 0 
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Figure  13.5  CFEKF  Range  Tracking,  + 2a 
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Time  (Minutes) 


Figure  13.6  CFEKF  Range  Tracking,  - 2 a 


SECTION  14 


MANEUVERING  TARGET  RESULTS 

All  the  results  in  the  previous  sections  utilized  a nonmaneuvering  target  even  when 
the  target  was  modeled  as  maneuvering.  The  question  naturally  arises  as  to  whether  target 
maneuvers  will  enhance  or  degrade  the  relative  performance  of  the  EKF,  MGEKF  and  the 
three  candidate  filters.  Another  simulation  set  was  run  to  demonstrate  the  effect  of  target 
maneuvers.  The  target  maneuver  pattern  was  chosen  to  be  as  follows.  The  initial  target 
heading  is  45  degrees  and  the  target  immediately  begins  to  maneuver  either  right  or  left  15 
degrees  depending  on  a random  number  selection  so  that  there  is  a .5  probability  of  the  target 
turning  in  either  direction.  The  target  turn  rate  is  limited  to  3 degrees/second  so  this  turn 
takes  5 seconds  to  accomplish.  Then  at  each  minute  mark  the  target  turns  30  degrees  in  the 
opposite  direction  of  the  last  turn  so  that  the  target  moves  in  a typical  zig-zag  pattern.  In 
this  manner  the  target  changes  its  x and  y velocities  by  about  3.66  knots  each  minute.  In 
section  5 it  was  noted  that  the  maneuvering  target  model  used  represents  a target  with  a la 
velocity  change  of  2.58  knots  each  minute.  Thus  the  target  manuevers  used  in  this  simulation 
set  actually  have  greater  "energy"  than  the  model  (in  the  sense  of  mean  squared  deviation 
from  the  nominal  course). 

The  simulation  results  for  the  five  filters  under  investigation  are  shown  on  the  following 
pages  and  show  the  same  general  trend  as  seen  in  previous  sections  with  the  MB  EKF  giving 
the  best  performance  in  terms  of  bias  and  la  error. 
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Figure  14.2  MGEKF  Range  Tracking,  Manuevering  Target 
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Time  (Minutes) 

Figure  14.3  MBEKF  Range  Tracking,  Manuevering  Target 
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SECTION  15 


SUMMARY  AND  CONCLUSIONS 

An  analysis  of  the  Extended  Kalman  Filter  as  applied  to  passive  target  tracking  has 
been  accomplished.  Using  a combination  of  empirical  and  analytical  techniques  the  exis- 
tence and  cause  of  an  estimation  bias  has  been  identified.  The  cause,  a correlation  between 
the  gain  and  innovations  sequences,  was  uncovered  using  simulation  analysis  of  the 
performance  of  the  EKF  which  motivated  the  subsequent  mathematical  investigation.  Under 
a set  of  assumptions  a stochastic  perturbation  analysis  of  the  state  update  equation  of  the 
EKF  yielded  a prediction  formula  for  this  bias.  It  was  shown  that  the  prediction  is  accurate 
under  the  assumptions  made  but  that  it  is  invalid  under  the  conditions  of  a realizable  filter. 
The  relationship  between  the  realizable  prediction  and  the  actual  bias  was  identified 
empirically. 

The  ultimate  goal  of  this  research  was  to  produce  an  estimator  or  estimators  that  would 
yield  optimal  performance  defined  as  near  minimum  variance  estimates  with  insignificant 
bias.  Three  methods  were  used  in  the  attempt.  The  first  method  used  an  estimate  of  the 
bias  to  remove  it  from  the  a posteriori  estimate  at  each  measurement  update.  Simulation 
results  using  this  method  showed  that  the  bias  was  removed  with  only  a trivial  increase  in 
the  actual  error  covariance.  Two  other  approaches  were  formulated  in  an  effort  to  prevent 
the  introduction  of  the  bias  in  the  first  place.  Both  methods  sought  to  remove  or  reduce  the 
bias  by  reducing  the  correlation  that  causes  it  and  were  at  least  moderately  successfully. 
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All  three  methods  show  considerable  improvement  over  the  standard  cartesian  EKF. 
The  most  important  aspect  of  all  these  approaches  is  that  the  heretofore  unidentified  cause 
of  the  poor  performance  of  the  EKF  has  been  utilized  in  the  filter  designs  as  opposed  to  the 
multitude  of  ad  hoc  techniques  used  previously.  Because  of  this  fundamental  advantage  of 
these  estimators  it  is  clear  that  the  improvement  in  estimation  performance  revealed  in  the 
simulation  results  is  of  a more  general  nature  than  previous  efforts. 
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NOMENCLATURE 
= Actual  State  Vector 
= Estimated  State  Vector 
= Error  in  a priori  State  Estimation  Vector 
= Relative  Position  Coordinates 
= Relative  Velocities 
= a priori  Position  Estimation  Errors 
= a priori  Velocity  Estimation  Errors 

= Change  in  Position  Estimates  due  to  State  Update  (a  posteriori 
estimate  minus  a priori  estimate) 

= Change  in  Velocity  Estimates  due  to  State  Update  {a  posteriori 
estimate  minus  a priori  estimate) 

= Actual  LOS  (Bearing)  Angle 

= Actual  Range 

= Estimated  Range 

= Measurement  of  LOS  Angle 

= Predicted  LOS  Angle  based  on  a priori  estimate 

= Error  in  Predicted  LOS  Angle 

= Change  in  Range  Estimate  due  to  State  Update  (a  posteriori 
estimate  minus  a priori  estimate) 
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= Actual  Range-rate 

= Change  in  Range-rate  Estimate  due  to  State  Update  (a posteriori 
estimate  minus  a priori  estimate) 

= Actual  Variance  of  Position  Estimate 

= Actual  Variance  of  Velocity  Estimate 

= Actual  Covariances  between  Components  of  State  Vector 

= Filter  Estimated  a priori  Covariance  between  estimated  states  x, 
and  Xj 

= Filter  Estimated  Variances  in  Polar  Coordinates  (First-Order 
Approximations) 

= Filter  Estimated  a priori  Covariances  in  Polar  Coordinates 
(First-Order  Approximations) 
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FIRST-ORDER  APPROXIMATIONS 

Here  are  the  first-order  approximations  used  to  convert  between  cartesian  and  polar 
coordinates. 
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Some  important  products, 

gQ2  y2&x2-2xy&x8y  +x28y2 


g^2  _ x2&y2  + 2xy&c5y  +y25y2 

J2 


8r  50  = 


-xyStx  +(;c2-y2)5x5y +xy5y2 


g^g^.  _ x28x8x  +xy(&r5y  + 8x5y)  + y25y5y 


Taking  the  expected  values  of  the  above  yields  approximations  for  the  actual 
covariances, 


£[8r2]  = o?  = 


x2a?  + 2xyo^  + y2o^ 


£[5r50]  = c^e« 


-xyc^  + (x2-y2)o^+xyc^ 


£[8r5r] 


.]  = a2  x^i+xyjoty  + ^ + y2^ 
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£[665 r]  = o?„  - 

r3 

We  also  define  mixed  terms  used  in  the  bias  deviation, 
a2f)  = H(x)MHT(x)  = 


yy 


77  x y2mn-2xymn+x2m22 


Z?  n,  N x mn  + 2xyml2+y  m22 

cr  = G(x)MG  (x)= 


z?  , -xymll  + (x2-y2)ml2+xym22 

°ro  ~ G(x)MH  (x)  = 


—2 


cr,  = J(x)MG‘(x)  = 


T x2ml2+xy(mu  + m22)  + y2m24 


Note  that  these  terms  are  functions  of  both  the  actual  geometric  variables  and  the  filter’s 
estimates  of  its  estimation  covariance. 

The  filter’s  estimate  of  its  own  a priori  estimation  covariance  in  polar  coordinates  are, 

Z?  tit  ''watiT/  fmn-lxymn+x2^ 

Gq  = H(x)MH  (x)= - 


or  = G(x)MG‘(x)  = 


T,  „ x2mn  + 2 xymn  + yVzz 


7?  ,Tf^  -xymn  + 0c2 - y2)mn + xym^ 

ar0  = G(x)MH  (x)  = 
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or,  = J(x)MG  (x)  = 
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